public void reshape(int numberOfCoefficientsRequired) { polynomial.reshape(numberOfCoefficientsRequired); tInitial.setToNaN(); tFinal.setToNaN(); }
public void setDirectly(double[] coefficients) { reshape(coefficients.length); for (int i = 0; i < numberOfCoefficients.getIntegerValue(); i++) { this.a[i].set(coefficients[i]); } }
public void setLinear(double t0, double tFinal, double z0, double zf) { reshape(2); setPositionRow(0, t0, z0); setPositionRow(1, tFinal, zf); solveForCoefficients(); setYoVariables(); }
public void setLinear(double t, double z, double zd) { reshape(2); setPositionRow(0, t, z); setVelocityRow(1, t, zd); solveForCoefficients(); setYoVariables(); }
public void setQuadraticUsingInitialAcceleration(double t0, double tFinal, double z0, double zd0, double zdd0) { reshape(3); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setAccelerationRow(2, t0, zdd0); solveForCoefficients(); setYoVariables(); }
public void setQuadraticWithFinalVelocityConstraint(double t0, double tFinal, double z0, double zFinal, double zdFinal) { reshape(3); setPositionRow(0, t0, z0); setPositionRow(1, tFinal, zFinal); setVelocityRow(2, tFinal, zdFinal); solveForCoefficients(); setYoVariables(); }
public void setCubic(double t0, double tFinal, double z0, double zd0, double zFinal, double zdFinal) { reshape(4); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setPositionRow(2, tFinal, zFinal); setVelocityRow(3, tFinal, zdFinal); solveForCoefficients(); setYoVariables(); }
public void setCubicThreeInitialConditionsFinalPosition(double t0, double tFinal, double z0, double zd0, double zdd0, double zFinal) { reshape(4); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setAccelerationRow(2, t0, zdd0); setPositionRow(3, tFinal, zFinal); solveForCoefficients(); setYoVariables(); }
public void setCubic(double t0, double tFinal, double z0, double zFinal) { reshape(4); setPositionRow(0, t0, z0); setVelocityRow(1, t0, 0.0); setPositionRow(2, tFinal, zFinal); setVelocityRow(3, tFinal, 0.0); solveForCoefficients(); setYoVariables(); }
public void setQuadraticUsingIntermediatePoint(double t0, double tIntermediate, double tFinal, double z0, double zIntermediate, double zFinal) { reshape(3); MathTools.checkIntervalContains(tIntermediate, t0, tFinal); setPositionRow(0, t0, z0); setPositionRow(1, tIntermediate, zIntermediate); setPositionRow(2, tFinal, zFinal); solveForCoefficients(); setYoVariables(); }
public void setQuadraticUsingIntermediatePoint(double t0, double tIntermediate, double tFinal, double z0, double zIntermediate, double zFinal) { reshape(3); MathTools.checkIfInRange(tIntermediate, t0, tFinal); setPositionRow(0, t0, z0); setPositionRow(1, tIntermediate, zIntermediate); setPositionRow(2, tFinal, zFinal); solveForCoefficients(); setYoVariables(); }
public void setCubicWithIntermediatePositionAndInitialVelocityConstraint(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zIntermediate, double zFinal) { reshape(4); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setPositionRow(2, tIntermediate, zIntermediate); setPositionRow(3, tFinal, zFinal); solveForCoefficients(); setYoVariables(); }
public void setCubicWithIntermediatePositionAndFinalVelocityConstraint(double t0, double tIntermediate, double tFinal, double z0, double zIntermediate, double zFinal, double zdFinal) { reshape(4); setPositionRow(0, t0, z0); setPositionRow(1, tIntermediate, zIntermediate); setPositionRow(2, tFinal, zFinal); setVelocityRow(3, tFinal, zdFinal); solveForCoefficients(); setYoVariables(); }
public void setQuarticUsingOneIntermediateVelocity(double t0, double tIntermediate0, double tIntermediate1, double tFinal, double z0, double zIntermediate0, double zIntermediate1, double zFinal, double zdIntermediate1) { reshape(5); setPositionRow(0, t0, z0); setPositionRow(1, tIntermediate0, zIntermediate0); setPositionRow(2, tIntermediate1, zIntermediate1); setVelocityRow(3, tIntermediate1, zdIntermediate1); setPositionRow(4, tFinal, zFinal); solveForCoefficients(); setYoVariables(); }
public void setQuarticUsingWayPoint(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zIntermediate, double zf, double zdf) { reshape(5); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setPositionRow(2, tIntermediate, zIntermediate); setPositionRow(3, tFinal, zf); setVelocityRow(4, tFinal, zdf); solveForCoefficients(); setYoVariables(); }
public void setQuinticUsingWayPoint2(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zdd0, double zIntermediate, double zdIntermediate, double zf) { reshape(6); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setAccelerationRow(2, t0, zdd0); setPositionRow(3, tIntermediate, zIntermediate); setVelocityRow(4, tIntermediate, zdIntermediate); setPositionRow(5, tFinal, zf); solveForCoefficients(); setYoVariables(); }
public void setQuintic(double t0, double tFinal, double z0, double zd0, double zdd0, double zf, double zdf, double zddf) { reshape(6); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setAccelerationRow(2, t0, zdd0); setPositionRow(3, tFinal, zf); setVelocityRow(4, tFinal, zdf); setAccelerationRow(5, tFinal, zddf); solveForCoefficients(); setYoVariables(); }
public void setQuinticUsingWayPoint(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zdd0, double zIntermediate, double zf, double zdf) { reshape(6); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setAccelerationRow(2, t0, zdd0); setPositionRow(3, tIntermediate, zIntermediate); setPositionRow(4, tFinal, zf); setVelocityRow(5, tFinal, zdf); solveForCoefficients(); setYoVariables(); }
public void setQuinticUsingIntermediateVelocityAndAcceleration(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zdIntermediate, double zddIntermediate, double zFinal, double zdFinal) { reshape(6); setPositionRow(0, t0, z0); setVelocityRow(1, t0, zd0); setVelocityRow(2, tIntermediate, zdIntermediate); setAccelerationRow(3, tIntermediate, zddIntermediate); setPositionRow(4, tFinal, zFinal); setVelocityRow(5, tFinal, zdFinal); solveForCoefficients(); setYoVariables(); }
public void getDerivative(YoTrajectory dervTraj, int order) { if (dervTraj.getMaximumNumberOfCoefficients() < this.getNumberOfCoefficients() - order) return; dervTraj.polynomial.reshape(getNumberOfCoefficients() - order); for (int i = order; i < this.getNumberOfCoefficients(); i++) { dervTraj.polynomial.setDirectlyFast(i - order, this.polynomial.getDerivativeCoefficient(order, i) * this.polynomial.getCoefficient(i)); } dervTraj.setInitialTime(this.getInitialTime()); dervTraj.setFinalTime(this.getFinalTime()); }