public void setQuintics(double[] times, FramePoint3D[] positions, FrameVector3D[] velocities, FrameVector3D[] accelerations) { MathTools.checkEquals(times.length, rangeList.size() + 1); MathTools.checkEquals(positions.length, times.length); MathTools.checkEquals(velocities.length, times.length); MathTools.checkEquals(accelerations.length, times.length); for (int i = 0; i < times.length - 1; i++) { YoSpline3D spline = splines.get(i); double t0 = times[i]; double tf = times[i + 1]; FramePoint3D p0 = positions[i]; FramePoint3D pf = positions[i + 1]; FrameVector3D pd0 = velocities[i]; FrameVector3D pdf = velocities[i + 1]; FrameVector3D pdd0 = accelerations[i]; FrameVector3D pddf = accelerations[i + 1]; spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf); rangeList.get(i).getLeft().set(t0); rangeList.get(i).getRight().set(tf); } setArcLength(); }
public void setQuintics(YoConcatenatedSplines oldSplines, double[] oldTimes, double[] newTimes) { MathTools.checkEquals(oldTimes.length, newTimes.length); MathTools.checkEquals(oldTimes.length, rangeList.size() + 1); for (int i = 0; i < oldTimes.length - 1; i++) { YoSpline3D spline = splines.get(i); oldSplines.compute(oldTimes[i]); double t0 = newTimes[i]; FramePoint3D p0 = oldSplines.getPosition(); FrameVector3D pd0 = oldSplines.getVelocity(); FrameVector3D pdd0 = oldSplines.getAcceleration(); oldSplines.compute(oldTimes[i + 1]); double tf = newTimes[i + 1]; FramePoint3D pf = oldSplines.getPosition(); FrameVector3D pdf = oldSplines.getVelocity(); FrameVector3D pddf = oldSplines.getAcceleration(); spline.setQuintic(t0, tf, p0, pd0, pdd0, pf, pdf, pddf); rangeList.get(i).getLeft().set(t0); rangeList.get(i).getRight().set(tf); } setArcLength(); }
public void setLinearUsingFinalPositionAndVelocity(double t0, double tf, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 2); pf.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setLinear(tf, pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setLinearUsingInitialPositionAndVelocity(double t0, double tf, FramePoint3D p0, FrameVector3D pd0) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 2); p0.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setLinear(t0, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setLinear(double t0, double tf, FramePoint3D p0, FramePoint3D pf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 2); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setLinear(t0, tf, p0.getElement(axis.ordinal()), pf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
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); }
public void setQuadraticUsingInitialVelocityAndAcceleration(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdd0) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 3); p0.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdd0.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuadraticUsingInitialAcceleration(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdd0.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setCubic(double x0, double x1, double y0, double yd0, double y1, double yd1) { MathTools.checkEquals(coefficients.length, 4); constraintMatrix = new DenseMatrix64F(new double[4][4]); constraintVector = new DenseMatrix64F(new double[4][1]); coefficientVector = new DenseMatrix64F(new double[4][1]); setPointConstraint(0, x0, y0); setDerivativeConstraint(1, x0, yd0); setPointConstraint(2, x1, y1); setDerivativeConstraint(3, x1, yd1); solve(constraintMatrix, constraintVector, coefficientVector); setVariables(); }
public void setCubic(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 4); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setCubic(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pf.getElement(axis.ordinal()), pdf.getElement( axis.ordinal())); } setYoVariables(t0, tf); }
public void setCubicUsingFinalAccelerationButNotFinalPosition(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdf, FrameVector3D pddf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 4); p0.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pddf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setCubicUsingFinalAccelerationButNotFinalPosition(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdf.getElement( axis.ordinal()), pddf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setQuintic(double x0, double x1, double y0, double yd0, double ydd0, double y1, double yd1, double ydd1) { MathTools.checkEquals(coefficients.length, 6); constraintMatrix = new DenseMatrix64F(new double[6][6]); constraintVector = new DenseMatrix64F(new double[6][1]); coefficientVector = new DenseMatrix64F(new double[6][1]); setPointConstraint(0, x0, y0); setDerivativeConstraint(1, x0, yd0); setDoubleDerivativeConstraint(2, x0, ydd0); setPointConstraint(3, x1, y1); setDerivativeConstraint(4, x1, yd1); setDoubleDerivativeConstraint(5, x1, ydd1); solve(constraintMatrix, constraintVector, coefficientVector); setVariables(); }
public void setQuarticUsingInitialAcceleration(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdd0, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 5); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuartic(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdd0.getElement(axis.ordinal()), pf.getElement( axis.ordinal()), pdf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setQuarticUsingFinalAcceleration(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FramePoint3D pf, FrameVector3D pdf, FrameVector3D pddf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 5); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pddf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuartic(tf, t0, pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal()), pddf.getElement(axis.ordinal()), p0.getElement( axis.ordinal()), pd0.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setQuarticUsingIntermediateVelocity(double t0, double t1, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pd1, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 5); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pd1.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuarticUsingIntermediateVelocity(t0, t1, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pd1.getElement( axis.ordinal()), pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setQuinticUsingIntermediateVelocityAndAcceleration(double t0, double t1, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pd1, FrameVector3D pdd1, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 6); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pd1.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pdd1.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuinticUsingIntermediateVelocityAndAcceleration(t0, t1, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pd1.getElement( axis.ordinal()), pdd1.getElement(axis.ordinal()), pf.getElement(axis.ordinal()), pdf.getElement( axis.ordinal())); } setYoVariables(t0, tf); }
public void setQuintic(double t0, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdd0, FramePoint3D pf, FrameVector3D pdf, FrameVector3D pddf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 6); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pddf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setQuintic(t0, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdd0.getElement(axis.ordinal()), pf.getElement( axis.ordinal()), pdf.getElement(axis.ordinal()), pddf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setSexticUsingWaypointVelocityAndAccelerationAndFinalAcceleration(double t0, double t1, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pd1, FrameVector3D pdd1, FramePoint3D pf, FrameVector3D pdf, FrameVector3D pddf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 7); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pd1.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pdd1.checkReferenceFrameMatch(referenceFrame); pddf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setSexticUsingWaypointVelocityAndAcceleration(tf, t1, t0, pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal()), pddf.getElement( axis.ordinal()), pd1.getElement(axis.ordinal()), pdd1.getElement(axis.ordinal()), p0.getElement( axis.ordinal()), pd0.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setSexticUsingWaypoint(double t0, double t1, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdd0, FramePoint3D p1, FramePoint3D pf, FrameVector3D pdf, FrameVector3D pddf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 7); p0.checkReferenceFrameMatch(referenceFrame); p1.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdd0.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pddf.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setSexticUsingWaypoint(t0, t1, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdd0.getElement(axis.ordinal()), p1.getElement( axis.ordinal()), pf.getElement(axis.ordinal()), pdf.getElement(axis.ordinal()), pddf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }
public void setSexticUsingWaypointVelocityAndAccelerationAndInitialAcceleration(double t0, double t1, double tf, FramePoint3D p0, FrameVector3D pd0, FrameVector3D pdd0, FrameVector3D pd1, FrameVector3D pdd1, FramePoint3D pf, FrameVector3D pdf) { MathTools.checkEquals(numberOfCoefficientsPerPolynomial, 7); p0.checkReferenceFrameMatch(referenceFrame); pf.checkReferenceFrameMatch(referenceFrame); pd0.checkReferenceFrameMatch(referenceFrame); pdd0.checkReferenceFrameMatch(referenceFrame); pd1.checkReferenceFrameMatch(referenceFrame); pdf.checkReferenceFrameMatch(referenceFrame); pdd1.checkReferenceFrameMatch(referenceFrame); for (Axis axis : Axis.values) { polynomials.get(axis).setSexticUsingWaypointVelocityAndAcceleration(t0, t1, tf, p0.getElement(axis.ordinal()), pd0.getElement(axis.ordinal()), pdd0.getElement( axis.ordinal()), pd1.getElement(axis.ordinal()), pdd1.getElement(axis.ordinal()), pf.getElement( axis.ordinal()), pdf.getElement(axis.ordinal())); } setYoVariables(t0, tf); }