public void getPosition(FramePoint positionToPack) { positionProvider.getPosition(positionToPack); } }
public void getPosition(FramePoint3D positionToPack) { positionProvider.getPosition(positionToPack); } }
private List<FramePoint> getWaypointsAtGroundClearance(double groundClearance, double[] proportionsThroughTrajectoryForGroundClearance) { FramePoint initialPosition = allPositions[0].getFramePointCopy(); FramePoint finalPosition = allPositions[3].getFramePointCopy(); positionSources[0].getPosition(initialPosition); positionSources[1].getPosition(finalPosition); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); List<FramePoint> waypoints = getWaypointsAtSpecifiedGroundClearance(initialPosition, finalPosition, groundClearance, proportionsThroughTrajectoryForGroundClearance); return waypoints; }
private void updateInitialPosition() { initialPositionProvider.getPosition(tempInitialPosition); tempInitialPosition.changeFrame(initialPosition.getReferenceFrame()); initialPosition.set(tempInitialPosition); }
private void updateFinalPosition() { finalPositionProvider.getPosition(tempFinalPosition); tempFinalPosition.changeFrame(finalPosition.getReferenceFrame()); finalPosition.set(tempFinalPosition); }
private void updateFinalPosition() { finalPositionProvider.getPosition(tempFinalPosition); tempFinalPosition.changeFrame(finalPosition.getReferenceFrame()); finalPosition.set(tempFinalPosition); }
private void updateInitialPosition() { initialPositionProvider.getPosition(tempInitialPosition); tempInitialPosition.changeFrame(initialPosition.getReferenceFrame()); initialPosition.set(tempInitialPosition); }
public void initialize() { time.set(0.0); FramePoint3D positionToPack = new FramePoint3D(); positionProvider.getPosition(positionToPack); positionToPack.changeFrame(position.getReferenceFrame()); position.set(positionToPack); }
public double getValue() { FramePoint3D initialPosition = new FramePoint3D(); initialPositionProvider.getPosition(initialPosition); initialPosition.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint3D finalPosition = new FramePoint3D(); finalPositionProvider.getPosition(finalPosition); finalPosition.changeFrame(ReferenceFrame.getWorldFrame()); double distance = initialPosition.distance(finalPosition); double time = distance / averageVelocityProvider.getValue(); time = MathTools.clamp(time, minimumTime, Double.POSITIVE_INFINITY); return time; } }
public double getValue() { FramePoint initialPosition = new FramePoint(); initialPositionProvider.getPosition(initialPosition); initialPosition.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint finalPosition = new FramePoint(); finalPositionProvider.getPosition(finalPosition); finalPosition.changeFrame(ReferenceFrame.getWorldFrame()); double distance = initialPosition.distance(finalPosition); double time = distance / averageVelocityProvider.getValue(); time = MathTools.clipToMinMax(time, minimumTime, Double.POSITIVE_INFINITY); return time; } }
public void initialize() { time.set(0.0); FramePoint positionToPack = new FramePoint(); positionProvider.getPosition(positionToPack); positionToPack.changeFrame(position.getReferenceFrame()); position.set(positionToPack); }
private List<FramePoint> getWaypointsAtGroundClearances(double[] groundClearances, double[] proportionsThroughTrajectoryForGroundClearance) { FramePoint initialPosition = allPositions[0].getFramePointCopy(); FramePoint finalPosition = allPositions[3].getFramePointCopy(); positionSources[0].getPosition(initialPosition); positionSources[1].getPosition(finalPosition); initialPosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); List<FramePoint> waypoints = new ArrayList<FramePoint>(); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); for (int i = 0; i < 2; i++) { FramePoint waypoint = waypoints.get(i); waypoint.set(initialPosition); FrameVector offsetFromInitial = new FrameVector(waypoint.getReferenceFrame()); offsetFromInitial.set(finalPosition); offsetFromInitial.sub(initialPosition); offsetFromInitial.scale(proportionsThroughTrajectoryForGroundClearance[i]); waypoint.add(offsetFromInitial); waypoint.setZ(waypoints.get(i).getZ() + groundClearances[i]); } return waypoints; }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); initialPositionProvider.getPosition(tempFramePoint); tempFramePoint.changeFrame(referenceFrame); double y = tempFramePoint.getY(); double x = tempFramePoint.getX(); z.set(tempFramePoint.getZ()); radius.set(Math.sqrt(x * x + y * y)); double initialAngle = Math.atan2(y, x); double finalAngle = initialAngle + desiredRotationAngleProvider.getValue(); anglePolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialAngle, 0.0, 0.0, finalAngle, 0.0, 0.0); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); initialPositionProvider.getPosition(tempFramePoint); tempFramePoint.changeFrame(referenceFrame); double y = tempFramePoint.getY(); double x = tempFramePoint.getX(); z.set(tempFramePoint.getZ()); radius.set(Math.sqrt(x * x + y * y)); double initialAngle = Math.atan2(y, x); double finalAngle = initialAngle + desiredRotationAngleProvider.getValue(); anglePolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialAngle, 0.0, 0.0, finalAngle, 0.0, 0.0); }
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); }
public void initialize() { timeIntoStep.set(0.0); this.stepTime.set(stepTimeProvider.getValue()); if (stepTime.getDoubleValue() < 1e-10) { stepTime.set(1e-10); } minimumJerkTrajectory.setParams(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, stepTime.getDoubleValue()); double middleOfTrajectoryParameter = 0.5; FramePoint3D initialPosition = new FramePoint3D(ReferenceFrame.getWorldFrame()); initialPositionProvider.getPosition(initialPosition); FramePoint3D finalPosition = new FramePoint3D(ReferenceFrame.getWorldFrame()); finalPositionProvider.getPosition(finalPosition); initialPosition.changeFrame(parabolicTrajectoryGenerator.getReferenceFrame()); finalPosition.changeFrame(parabolicTrajectoryGenerator.getReferenceFrame()); double maxAnkleHeight = Math.max(initialPosition.getZ(), finalPosition.getZ()); double apexHeight = maxAnkleHeight + groundClearance.getDoubleValue(); parabolicTrajectoryGenerator.initialize(initialPosition, finalPosition, apexHeight, middleOfTrajectoryParameter); }
public void initialize() { timeIntoStep.set(0.0); this.stepTime.set(stepTimeProvider.getValue()); if (stepTime.getDoubleValue() < 1e-10) { stepTime.set(1e-10); } minimumJerkTrajectory.setParams(0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, stepTime.getDoubleValue()); double middleOfTrajectoryParameter = 0.5; FramePoint initialPosition = new FramePoint(ReferenceFrame.getWorldFrame()); initialPositionProvider.getPosition(initialPosition); FramePoint finalPosition = new FramePoint(ReferenceFrame.getWorldFrame()); finalPositionProvider.getPosition(finalPosition); initialPosition.changeFrame(parabolicTrajectoryGenerator.getReferenceFrame()); finalPosition.changeFrame(parabolicTrajectoryGenerator.getReferenceFrame()); double maxAnkleHeight = Math.max(initialPosition.getZ(), finalPosition.getZ()); double apexHeight = maxAnkleHeight + groundClearance.getDoubleValue(); parabolicTrajectoryGenerator.initialize(initialPosition, finalPosition, apexHeight, middleOfTrajectoryParameter); }
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()); }
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(); } }
FramePoint3D actual = new FramePoint3D(worldFrame); FramePoint3D expected = new FramePoint3D(worldFrame); initialPositionProvider.getPosition(expected); trajectory.getPosition(actual); assertEquals(actual.getX(), expected.getX(), 1e-7);