public void setQuadratic(double t0, double tFinal, double z0, double zd0, double zFinal) { setTime(t0, tFinal); polynomial.setQuadratic(t0, tFinal, z0, zd0, zFinal); }
public void setQuadraticUsingInitialVelocity(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 3); for (Direction direction : Direction.values) { polynomials.get(direction).setQuadratic(t0, tf, p0.get(direction), pd0.get(direction), pf.get(direction)); } setYoVariables(t0, tf); }
public void setQuadraticUsingFinalVelocity(double t0, double tf, FramePoint p0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 3); for (Direction direction : Direction.values) { polynomials.get(direction).setQuadratic(tf, t0, pf.get(direction), pdf.get(direction), p0.get(direction)); } setYoVariables(t0, tf); }
public void setQuadratic(double t0, double tFinal, Point3DReadOnly z0, Vector3DReadOnly zd0, Point3DReadOnly zFinal) { for (int index = 0; index < 3; index++) getYoPolynomial(index).setQuadratic(t0, tFinal, z0.getElement(index), zd0.getElement(index), zFinal.getElement(index)); }
protected void setPolynomial() { polynomial.setQuadratic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), finalPositionProvider.getValue()); }
public FootstepPlanningResult planWaypoints() { waypoints.clear(); double yaw = bodyStartPose.getYaw(); xPoly.setQuadratic(0.0, 1.0, bodyStartPose.getX(), Math.cos(yaw) * 0.2, bodyGoalPose.getX()); yPoly.setQuadratic(0.0, 1.0, bodyStartPose.getY(), Math.sin(yaw) * 0.2, bodyGoalPose.getY()); zPoly.setQuadratic(0.0, 1.0, bodyStartPose.getZ(), Math.sin(yaw) * 0.2, bodyGoalPose.getZ()); for (int i = 0; i < numberOfPoints; i++) { double percent = i / (double) (numberOfPoints - 1); xPoly.compute(percent); yPoly.compute(percent); zPoly.compute(percent); Point3D point = new Point3D(xPoly.getPosition(), yPoly.getPosition(), zPoly.getPosition()); waypoints.add(point); } yoResult.set(FootstepPlanningResult.OPTIMAL_SOLUTION); return yoResult.getEnumValue(); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); polynomial.setQuadratic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), finalPositionProvider.getValue()); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); polynomial.setQuadratic(0.0, trajectoryTime.getDoubleValue(), initialPositionProvider.getValue(), initialVelocityProvider.getValue(), finalPositionProvider.getValue()); }
public void setQuadraticUsingInitialVelocity(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FramePoint3D pf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 3); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuadratic(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setQuadraticUsingFinalVelocity(double t0, double tf, FramePoint3D p0, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 3); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuadratic(tf, t0, pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal()), p0.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
xPoly.setQuadratic(0.0, 1.0, 0.0, 0.2, x.getDoubleValue()); yPoly.setQuadratic(0.0, 1.0, 0.0, 0.0, y.getDoubleValue()); for (int i = 0; i < numberOfPoints; i++)