public void set(Trajectory3D trajToCopy) { for (int index = 0; index < 3; index++) getYoTrajectory(index).set(trajToCopy.getTrajectory(index)); } }
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()); }
@Override public double getAcceleration() { return isDone() ? 0.0 : findCurrentPolynomial().getAcceleration(); }
public void set(Trajectory3D other) { xTrajectory.set(other.getTrajectoryX()); yTrajectory.set(other.getTrajectoryY()); zTrajectory.set(other.getTrajectoryZ()); }
public void set(YoFramePolynomial3D other) { setReferenceFrame(other.getReferenceFrame()); xPolynomial.set(other.getYoPolynomialX()); yPolynomial.set(other.getYoPolynomialY()); zPolynomial.set(other.getYoPolynomialZ()); }
public void initialize() { positionTrajectoryInput.initialize(); setRawData(); }
@Override public void changeFrame(ReferenceFrame referenceFrame) { super.changeFrame(referenceFrame); initializePolynomials(); }
public void setSeptic(double t0, double tIntermediate0, double tIntermediate1, double tFinal, double z0, double zd0, double zIntermediate0, double zdIntermediate0, double zIntermediate1, double zdIntermediate1, double zf, double zdf) { setTime(t0, tFinal); polynomial.setSeptic(t0, tIntermediate0, tIntermediate1, tFinal, z0, zd0, zIntermediate0, zdIntermediate0, zIntermediate1, zdIntermediate1, zf, zdf); }
public void setSepticInitialAndFinalAcceleration(double t0, double tIntermediate0, double tIntermediate1, double tFinal, double z0, double zd0, double zdd0, double zIntermediate0, double zIntermediate1, double zf, double zdf, double zddf) { setTime(t0, tFinal); polynomial.setSepticInitialAndFinalAcceleration(t0, tIntermediate0, tIntermediate1, tFinal, z0, zd0, zdd0, zIntermediate0, zIntermediate1, zf, zdf, zddf); }
public void setSexticUsingWaypointVelocityAndAcceleration(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zdd0, double zdIntermediate, double zddIntermediate, double zFinal, double zdFinal) { setTime(t0, tFinal); polynomial.setSexticUsingWaypointVelocityAndAcceleration(t0, tIntermediate, tFinal, z0, zd0, zdd0, zdIntermediate, zddIntermediate, zFinal, zdFinal); }
public void setQuarticUsingWayPoint(double t0, double tIntermediate, double tFinal, double z0, double zd0, double zIntermediate, double zf, double zdf) { setTime(t0, tFinal); polynomial.setQuarticUsingWayPoint(t0, tIntermediate, tFinal, z0, zd0, zIntermediate, zf, zdf); }
public void setQuadraticWithFinalVelocityConstraint(double t0, double tFinal, double z0, double zFinal, double zdFinal) { setTime(t0, tFinal); polynomial.setQuadraticWithFinalVelocityConstraint(t0, tFinal, z0, zFinal, zdFinal); }
public void setQuadraticUsingInitialAcceleration(double t0, double tFinal, double z0, double zd0, double zdd0) { setTime(t0, tFinal); polynomial.setQuadraticUsingInitialAcceleration(t0, tFinal, z0, zd0, zdd0); }
public void reset() { for (int index = 0; index < 3; index++) getTrajectory(index).reset(); }
public void update(double timeInState, FramePoint3D desiredPositionToPack, FrameVector3D desiredVelocityToPack) { update(timeInState, desiredPositionToPack); currentSegment.getFrameVelocity(desiredVelocityToPack); }
public void setCubicBezier(double t0, double tFinal, FramePoint3D z0, FramePoint3D zR1, FramePoint3D zR2, FramePoint3D zFinal) { checkReferenceFrameMatch(z0); checkReferenceFrameMatch(zR1); checkReferenceFrameMatch(zR2); checkReferenceFrameMatch(zFinal); super.setCubicBezier(t0, tFinal, z0, zR1, zR2, zFinal); }
public void setQuinticTwoWaypoints(double t0, double tIntermediate0, double tIntermediate1, double tFinal, double z0, double zd0, double zIntermediate0, double zIntermediate1, double zf, double zdf) { setTime(t0, tFinal); polynomial.setQuinticTwoWaypoints(t0, tIntermediate0, tIntermediate1, tFinal, z0, zd0, zIntermediate0, zIntermediate1, zf, zdf); }
public void setCubicBezier(double t0, double tFinal, double z0, double zR1, double zR2, double zFinal) { setTime(t0, tFinal); polynomial.setCubicBezier(t0, tFinal, z0, zR1, zR2, zFinal); }