@Override public void getVector(Vector3d vectorToPack) { this.get(vectorToPack); }
public void setIncludingFrame(FrameVector force, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); }
/** * Sets the angular part of the twist */ public void setAngularPart(FrameVector angularPart) { expressedInFrame.checkReferenceFrameMatch(angularPart); angularPart.get(this.angularPart); }
public void setSexticUsingWaypointVelocityAndAccelerationAndInitialAcceleration(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FrameVector pd1, FrameVector pdd1, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypointVelocityAndAcceleration(t0, t1, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pd1.get(direction), pdd1.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setSexticUsingWaypointVelocityAndAccelerationAndFinalAcceleration(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pd1, FrameVector pdd1, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypointVelocityAndAcceleration(tf, t1, t0, pf.get(direction), pdf.get(direction), pddf.get(direction), pd1.get(direction), pdd1.get(direction), p0.get(direction), pd0.get(direction)); } setYoVariables(t0, tf); }
public void setQuinticUsingIntermediateVelocityAndAcceleration(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pd1, FrameVector pdd1, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 6); for (Direction direction : Direction.values) { polynomials.get(direction).setQuinticUsingIntermediateVelocityAndAcceleration(t0, t1, tf, p0.get(direction), pd0.get(direction), pd1.get(direction), pdd1.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setQuintic(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 6); for (Direction direction : Direction.values) { polynomials.get(direction).setQuintic(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pf.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
public void setSexticUsingWaypoint(double t0, double t1, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint p1, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 7); for (Direction direction : Direction.values) { polynomials.get(direction).setSexticUsingWaypoint(t0, t1, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), p1.get(direction), pf.get(direction), pdf.get(direction), pddf.get(direction)); } setYoVariables(t0, tf); }
public void setQuarticUsingInitialAcceleration(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 5); for (Direction direction : Direction.values) { polynomials.get(direction).setQuartic(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
public void setQuarticUsingFinalAcceleration(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf, FrameVector pdf, FrameVector pddf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 5); for (Direction direction : Direction.values) { polynomials.get(direction).setQuartic(tf, t0, pf.get(direction), pdf.get(direction), pddf.get(direction), p0.get(direction), pd0.get(direction)); } setYoVariables(t0, tf); }
public void setQuadraticUsingInitialVelocityAndAcceleration(double t0, double tf, FramePoint p0, FrameVector pd0, FrameVector pdd0) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 3); for (Direction direction : Direction.values) { polynomials.get(direction).setQuadraticUsingInitialAcceleration(t0, tf, p0.get(direction), pd0.get(direction), pdd0.get(direction)); } setYoVariables(t0, tf); }
public void set(FramePoint desiredPosition, FrameVector desiredLinearVelocity, FrameVector feedForwardLinearAcceleration) { desiredPosition.checkReferenceFrameMatch(worldFrame); desiredLinearVelocity.checkReferenceFrameMatch(worldFrame); feedForwardLinearAcceleration.checkReferenceFrameMatch(worldFrame); desiredPosition.get(desiredPositionInWorld); desiredLinearVelocity.get(desiredLinearVelocityInWorld); feedForwardLinearAcceleration.get(feedForwardLinearAccelerationInWorld); }
public void set(FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration) { desiredOrientation.checkReferenceFrameMatch(worldFrame); desiredAngularVelocity.checkReferenceFrameMatch(worldFrame); feedForwardAngularAcceleration.checkReferenceFrameMatch(worldFrame); desiredOrientation.getQuaternion(desiredOrientationInWorld); desiredAngularVelocity.get(desiredAngularVelocityInWorld); feedForwardAngularAcceleration.get(feedForwardAngularAccelerationInWorld); }
public void setCubic(double t0, double tf, FramePoint p0, FrameVector pd0, FramePoint pf, FrameVector pdf) { MathTools.checkIfEqual(numberOfCoefficientsPerPolynomial, 4); for (Direction direction : Direction.values) { polynomials.get(direction).setCubic(t0, tf, p0.get(direction), pd0.get(direction), pf.get(direction), pdf.get(direction)); } setYoVariables(t0, tf); }
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 set(RigidBody rigidBody, FramePoint measurementPointInBodyFrame, FrameVector velocityOfMeasurementPointInWorldFrame, boolean isPointVelocityValid) { this.rigidBodyName = rigidBody.getName(); this.bodyFixedReferenceFrameName = measurementPointInBodyFrame.getReferenceFrame().getName(); this.isPointVelocityValid = isPointVelocityValid; measurementPointInBodyFrame.get(this.measurementPointInBodyFrame); velocityOfMeasurementPointInWorldFrame.get(this.velocityOfMeasurementPointInWorldFrame); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(vector); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); frameVector.setIncludingFrame(currentReferenceFrame, vector); frameVector.changeFrame(desiredFrame); frameVector.get(vector); set(vector); }
public void run() { centerOfMassAccelerationCalculator.getCoMAcceleration(comAccelerationFrameVector); comAccelerationFrameVector.changeFrame(ReferenceFrame.getWorldFrame()); comAccelerationFrameVector.get(comAcceleration); if (CORRUPT_DESIRED_ACCELERATIONS) signalCorruptor.corrupt(comAcceleration); comAccelerationFrameVector.set(comAcceleration); desiredCoMAndAngularAccelerationDataSource.setDesiredCenterOfMassAcceleration(comAccelerationFrameVector); } }