public void getVector(FrameVector3DBasics frameVector3D) { frameVector3D.setIncludingFrame(vector); }
/** * Tests on a per component basis if this vector is equal to the given {@code other} to an * {@code epsilon}. * <p> * If the two vectors have different frames, this method returns {@code false}. * </p> * * @param other the other vector to compare against this. Not modified. * @param epsilon the tolerance to use when comparing each component. * @return {@code true} if the two vectors are equal and are expressed in the same reference frame, * {@code false} otherwise. */ @Override public boolean epsilonEquals(FrameVector3D other, double epsilon) { return FrameVector3DBasics.super.epsilonEquals(other, epsilon); }
/** * Tests if {@code this} and {@code other} represent the same vector 3D to an {@code epsilon}. * <p> * Two vectors are considered geometrically equal if they are at a distance of less than or equal to * {@code epsilon}. * </p> * <p> * Note that {@code this.geometricallyEquals(other, epsilon) == true} does not necessarily imply * {@code this.epsilonEquals(other, epsilon)} and vice versa. * </p> * * @param other the other vector 3D to compare against this. Not modified. * @param epsilon the maximum distance that the two vectors can be spaced and still considered * equal. * @return {@code true} if the two vectors represent the same geometry, {@code false} otherwise. * @throws ReferenceFrameMismatchException if {@code other} is not expressed in the same reference * frame as {@code this}. */ @Override public boolean geometricallyEquals(FrameVector3D other, double epsilon) { return FrameVector3DBasics.super.geometricallyEquals(other, epsilon); }
linearJointTerm.setIncludingFrame(measurementFrame, 3, jointAccelerationTerm); linearConvectiveTerm.setIncludingFrame(measurementFrame, 3, convectiveTerm); centrifugalTerm.setToZero(measurementFrame); sensorAngularVelocity.setIncludingFrame(sensorTwist.getAngularPart()); sensorLinearVelocity.setIncludingFrame(sensorTwist.getLinearPart()); centrifugalTerm.cross(sensorAngularVelocity, sensorLinearVelocity); gravityTerm.setIncludingFrame(ReferenceFrame.getWorldFrame(), 0.0, 0.0, -robotState.getGravity()); gravityTerm.changeFrame(measurementFrame); residualToPack.set(0, measurement.getX() - linearJointTerm.getX() - linearConvectiveTerm.getX() - centrifugalTerm.getX() - gravityTerm.getX()); residualToPack.set(1, measurement.getY() - linearJointTerm.getY() - linearConvectiveTerm.getY() - centrifugalTerm.getY() - gravityTerm.getY()); residualToPack.set(2, measurement.getZ() - linearJointTerm.getZ() - linearConvectiveTerm.getZ() - centrifugalTerm.getZ() - gravityTerm.getZ());
/** * Calculates the linear velocity resulting from this twist but measured at a given position * {@code observerPosition} on the body. * <p> * Effectively, the resulting linear velocity ν<sub>result</sub> is calculated as follows: * * <pre> * ν<sub>result</sub> = ν<sub>this</sub> - P × ω<sub>this</sub> * </pre> * * where ω and ν represent the angular and linear parts respectively, and {@code P} is * the {@code observerPosition}. * </p> * * @param observerPosition the position on the body where the linear velocity is to be estimated. * Not modified. * @param linearVelocityToPack the vector used to store the result. Modified. * @throws ReferenceFrameMismatchException if {@code observerPosition} is not expressed in the * same reference frame as {@code this}. */ default void getLinearVelocityAt(FramePoint3DReadOnly observerPosition, FrameVector3DBasics linearVelocityToPack) { observerPosition.checkReferenceFrameMatch(getReferenceFrame()); linearVelocityToPack.setIncludingFrame(observerPosition); linearVelocityToPack.cross((Vector3DReadOnly) getAngularPart(), (Vector3DReadOnly) linearVelocityToPack); linearVelocityToPack.add(getLinearPart()); }
/** * Computes and packs the center of mass velocity for the given joint velocities. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointVelocityMatrix the matrix containing the joint velocities to use. Not modified. * @param centerOfMassVelocityToPack the vector used to stored the computed center of mass velocity. * Modified. */ public void getCenterOfMassVelocity(DenseMatrix64F jointVelocityMatrix, FrameVector3DBasics centerOfMassVelocityToPack) { CommonOps.mult(getCentroidalMomentumMatrix(), jointVelocityMatrix, momentumMatrix); centerOfMassVelocityToPack.setIncludingFrame(matrixFrame, 3, momentumMatrix); centerOfMassVelocityToPack.scale(1.0 / getTotalMass()); }
checkReferenceFrameMatch(bodyTwist); linearAccelerationToPack.setIncludingFrame(bodyTwist.getLinearPart()); // v linearAccelerationToPack.cross(bodyTwist.getAngularPart(), linearAccelerationToPack); // w x ((w x p) + v) linearAccelerationToPack.add(getLinearPart()); // vDot + (wDot x p) + w x ((w x p) + v)
/** * Computes and packs the center of mass velocity for the given joint velocities. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointVelocityMatrix the matrix containing the joint velocities to use. Not modified. * @param centerOfMassVelocityToPack the vector used to stored the computed center of mass velocity. * Modified. */ public void getCenterOfMassVelocity(DenseMatrix64F jointVelocityMatrix, FrameVector3DBasics centerOfMassVelocityToPack) { CommonOps.mult(getCentroidalMomentumMatrix(), jointVelocityMatrix, momentumMatrix); centerOfMassVelocityToPack.setIncludingFrame(matrixFrame, 3, momentumMatrix); centerOfMassVelocityToPack.scale(1.0 / getTotalMass()); }
/** * Computes and packs the center of mass acceleration for the given joint accelerations. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointAccelerationMatrix the matrix containing the joint accelerations to use. Not * modified. * @param centerOfMassAccelerationToPack the vector used to stored the computed center of mass * acceleration. Modified. */ public void getCenterOfMassAcceleration(DenseMatrix64F jointAccelerationMatrix, FrameVector3DBasics centerOfMassAccelerationToPack) { CommonOps.mult(getCentroidalMomentumMatrix(), jointAccelerationMatrix, momentumMatrix); CommonOps.addEquals(momentumMatrix, getBiasSpatialForceMatrix()); centerOfMassAccelerationToPack.setIncludingFrame(matrixFrame, 3, momentumMatrix); centerOfMassAccelerationToPack.scale(1.0 / getTotalMass()); }
public void getVector(FrameVector3DBasics frameVector3D) { frameVector3D.setIncludingFrame(vector); }
/** * Computes and packs the center of mass velocity for the given joint velocities. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointVelocityMatrix the matrix containing the joint velocities to use. Not modified. * @param centerOfMassVelocityToPack the vector used to stored the computed center of mass velocity. * Modified. */ public void getCenterOfMassVelocity(DenseMatrix64F jointVelocityMatrix, FrameVector3DBasics centerOfMassVelocityToPack) { CommonOps.mult(getJacobianMatrix(), jointVelocityMatrix, centerOfMassVelocityMatrix); centerOfMassVelocityToPack.setIncludingFrame(jacobianFrame, centerOfMassVelocityMatrix); }
checkReferenceFrameMatch(bodyTwist); linearAccelerationToPack.setIncludingFrame(getLinearPart()); MecanoTools.addCrossToVector(bodyTwist.getAngularPart(), bodyTwist.getLinearPart(), linearAccelerationToPack);