public void getPosition(FramePoint positionToPack) { positionProvider.getPosition(positionToPack); } }
public CubicPolynomialTrajectoryGenerator(String namePrefix, DoubleProvider initialPositionProvider, DoubleProvider finalPositionProvider, DoubleProvider trajectoryTimeProvider, YoVariableRegistry parentRegistry) { this(namePrefix, initialPositionProvider, new ConstantDoubleProvider(0.0), finalPositionProvider, new ConstantDoubleProvider(0.0), trajectoryTimeProvider, parentRegistry); }
public void getOrientation(FrameQuaternion orientationToPack) { orientationProvider.getOrientation(orientationToPack); }
private CirclePositionTrajectoryGenerator createTrajectory(ReferenceFrame referenceFrame, FramePoint3D offset, double rotationAngle, double trajectoryTime) { PositionProvider initialPositionProvider = new ConstantPositionProvider(offset); DoubleProvider desiredRotationAngleProvider = new ConstantDoubleProvider(rotationAngle); YoVariableRegistry parentRegistry = new YoVariableRegistry("CirclePositionTrajectoryGeneratorTest"); return new CirclePositionTrajectoryGenerator("", referenceFrame, new ConstantDoubleProvider(trajectoryTime), initialPositionProvider, parentRegistry, desiredRotationAngleProvider); } }
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 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); }
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; } }
@Before public void setUp() { referenceFrame = ReferenceFrame.constructARootFrame("rootFrameTEST"); orientation = new FrameQuaternion(referenceFrame); trajectoryTimeProvider = new SettableDoubleProvider(trajectoryTime); initialOrientationProvider = new ConstantOrientationProvider(orientation); finalOrientationProvider = new ConstantOrientationProvider(orientation); parentRegistry = new YoVariableRegistry("registry"); }
public void requestTouchdownForDisturbanceRecovery() { if (isPerformingTouchdown.getBooleanValue()) return; positionTrajectoryGenerator.getPosition(desiredPosition); currentDesiredFootPosition.set(desiredPosition); touchdownInitialTimeProvider.setValue(getTimeInCurrentState()); positionTrajectoryForDisturbanceRecovery.initialize(); isPerformingTouchdown.set(true); }
public double getDesired(T controlSpace) { return desiredQs.get(controlSpace).getValue(); }
public double timeInCurrentState() { return time.getValue() - switchTimeYoVariable.getDoubleValue(); }
private void setTrajectoryParameters() { this.trajectoryParameters = trajectoryParametersProvider.getTrajectoryParameters(); }
@Before public void setUp() { parentRegistry = new YoVariableRegistry("parentRegistryTEST"); referenceFrame = ReferenceFrame.constructARootFrame("rootNameTEST"); position = new FramePoint3D(referenceFrame, xValue, yValue, zValue); initialPositionProvider = new ConstantPositionProvider(position); finalPositionProvider = new ConstantPositionProvider(position); trajectoryTimeProvider = new ConstantDoubleProvider(10.0); }
public CubicPolynomialTrajectoryGenerator(String namePrefix, DoubleProvider initialPositionProvider, DoubleProvider finalPositionProvider, DoubleProvider trajectoryTimeProvider, YoVariableRegistry parentRegistry) { this(namePrefix, initialPositionProvider, new ConstantDoubleProvider(0.0), finalPositionProvider, new ConstantDoubleProvider(0.0), trajectoryTimeProvider, parentRegistry); }
public void getOrientation(FrameOrientation orientationToPack) { orientationProvider.getOrientation(orientationToPack); }
public void getPosition(FramePoint3D positionToPack) { positionProvider.getPosition(positionToPack); } }
public double getDesiredVelocity(T controlSpace) { return desiredQds.get(controlSpace).getValue(); }
@Before public void setUp() { initialPositionProvider = new ConstantDoubleProvider(0.0); trajectoryTimeProvider = new ConstantDoubleProvider(timeRequired); initialVelocityProvider = new ConstantDoubleProvider(INITIALVELOCITY); }
public void getOrientation(FrameOrientation orientationToPack) { orientationProvider.getOrientation(orientationToPack); orientationToPack.changeFrame(referenceFrame); }
public void getOrientation(FrameQuaternion orientationToPack) { orientationProvider.getOrientation(orientationToPack); orientationToPack.changeFrame(referenceFrame); }