@Override public void getPoint(Point3d pointToPack) { this.get(pointToPack); }
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); }
public void setPoints(FramePoint pointA, FramePoint pointB, FramePoint pointC) { pointA.checkReferenceFrameMatch(referenceFrame); pointB.checkReferenceFrameMatch(referenceFrame); pointC.checkReferenceFrameMatch(referenceFrame); pointA.get(temporaryPointA); pointB.get(temporaryPointB); pointC.get(temporaryPointC); plane3d.setPoints(temporaryPointA, temporaryPointB, temporaryPointC); }
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); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { desiredHoldOrientation.getQuaternion(localQuaternion); desiredSolePosition.get(localTranslation); transformToParent.set(localQuaternion, localTranslation); } };
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 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 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 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 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 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 setIncludingFrame(FrameVector force, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); }
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(FramePoint desiredPosition, FrameVector desiredLinearVelocity, FrameVector feedForwardLinearAcceleration) { desiredPosition.checkReferenceFrameMatch(worldFrame); desiredLinearVelocity.checkReferenceFrameMatch(worldFrame); feedForwardLinearAcceleration.checkReferenceFrameMatch(worldFrame); desiredPosition.get(desiredPositionInWorld); desiredLinearVelocity.get(desiredLinearVelocityInWorld); feedForwardLinearAcceleration.get(feedForwardLinearAccelerationInWorld); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(point); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); framePoint.setIncludingFrame(currentReferenceFrame, point); framePoint.changeFrame(desiredFrame); framePoint.get(point); set(point); }
/** * Creates a transform that transforms to the given point and rotates to make the z axis align with the * normal vector. */ private static RigidBodyTransform createTransformFromPointAndZAxis(FramePoint point, FrameVector zAxis) { RigidBodyTransform ret = new RigidBodyTransform(); ret.setRotation(GeometryTools.getAxisAngleFromZUpToVector(zAxis.getVectorCopy())); Vector3d translation = new Vector3d(); point.get(translation); ret.setTranslation(translation); return ret; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { positionToPointAt.get(xAxis); xAxis.setZ(0.0); xAxis.normalize(); yAxis.cross(zAxis, xAxis); rotationMatrix.setColumn(0, xAxis); rotationMatrix.setColumn(1, yAxis); rotationMatrix.setColumn(2, zAxis); transformToParent.setRotation(rotationMatrix); } }