public void setInitialTimePositionsAndVelocities() { t0 = startTimeProvider.getValue(); startTime.set(startTimeProvider.getValue()); timeIntoTouchdown.set(0.0); initialPositionSource.getPosition(p0); p0.changeFrame(referenceFrame); velocitySource.get(pd0); pd0.changeFrame(referenceFrame); accelerationSource.get(pdd0); pdd0.changeFrame(referenceFrame); }
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); }
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); }
private void setInitialAndFinalTimesPositionsAndVelocities() { FramePoint tempPosition = new FramePoint(referenceFrame); FrameVector tempVelocity = new FrameVector(referenceFrame); for (int i = 0; i < 2; i++) { positionSources[i].getPosition(tempPosition); velocitySources[i].get(tempVelocity); boolean setInitialVelocityToZero = setInitialSwingVelocityToZero.getBooleanValue(); if ((i == 0) && setInitialVelocityToZero) { tempVelocity.set(0.0, 0.0, 0.0); } tempPosition.changeFrame(referenceFrame); tempVelocity.changeFrame(referenceFrame); allPositions[endpointIndices[i]].set(tempPosition); allVelocities[endpointIndices[i]].set(tempVelocity); } if (stancePositionSource != null) { stancePositionSource.getPosition(tempPosition); tempPosition.changeFrame(referenceFrame); stancePosition.set(tempPosition); } allTimes[endpointIndices[0]].set(0.0); allTimes[endpointIndices[1]].set(stepTime.getDoubleValue()); }
FrameVector3D expectedVel = new FrameVector3D(worldFrame); trajectory.getVelocity(actualVel); initialVelocityProvider.get(expectedVel); assertEquals(actualVel.getX(), expectedVel.getX(), 1e-7); assertEquals(actualVel.getY(), expectedVel.getY(), 1e-7); finalVelocityProvider.get(expectedVel); assertEquals(actualVel.getX(), expectedVel.getX(), 1e-7); assertEquals(actualVel.getY(), expectedVel.getY(), 1e-7);
FrameVector3D expectedVel = new FrameVector3D(worldFrame); trajectory.getVelocity(actualVel); initialVelocityProvider.get(expectedVel); assertEquals(actualVel.getX(), expectedVel.getX(), 1e-7); assertEquals(actualVel.getY(), expectedVel.getY(), 1e-7); finalVelocityProvider.get(expectedVel); assertEquals(actualVel.getX(), expectedVel.getX(), 1e-7); assertEquals(actualVel.getY(), expectedVel.getY(), 1e-7); assertEquals(actual.getY(), expected.getY(), 1e-7); intermediateVelocityProvider.get(expectedVel); pushRecoveryTrajectoryGenerator.getVelocity(actualVel); assertEquals(actualVel.getX(), expectedVel.getX(), 1e-7);
public void initialize() { swingTime.set(swingTimeProvider.getValue()); timeIntoStep.set(swingTime.getDoubleValue() - swingTimeRemainingProvider.getValue()); positionSources[0].getPosition(tempPosition); tempPosition.changeFrame(desiredPosition.getReferenceFrame()); double x0 = tempPosition.getX(); double y0 = tempPosition.getY(); velocitySources[0].get(tempVector); tempVector.changeFrame(desiredPosition.getReferenceFrame()); double xd0 = tempVector.getX(); double yd0 = tempVector.getY(); positionSources[1].getPosition(tempPosition); tempPosition.changeFrame(desiredPosition.getReferenceFrame()); double xFinal = tempPosition.getX(); double yFinal = tempPosition.getY(); nominalTrajectoryGenerator.compute(timeIntoStep.getDoubleValue()); nominalTrajectoryGenerator.getAcceleration(tempVector); tempVector.changeFrame(desiredPosition.getReferenceFrame()); double xdd0 = tempVector.getX(); double ydd0 = tempVector.getY(); xPolynomial.setQuintic(timeIntoStep.getDoubleValue(), swingTime.getDoubleValue(), x0, xd0, xdd0, xFinal, 0.0, 0.0); yPolynomial.setQuintic(timeIntoStep.getDoubleValue(), swingTime.getDoubleValue(), y0, yd0, ydd0, yFinal, 0.0, 0.0); if (VISUALIZE) { visualizeTrajectory(); } }
touchdownVelocityProvider.get(finalVelocity); swingTrajectoryGeneratorNew.setFinalConditions(finalPosition, finalVelocity);