public void getVector(FrameVector3DBasics frameVector3D) { frameVector3D.setIncludingFrame(vector); }
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); }
/** * 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 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()); }
/** * 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()); }
checkReferenceFrameMatch(bodyTwist); linearAccelerationToPack.setIncludingFrame(getLinearPart()); MecanoTools.addCrossToVector(bodyTwist.getAngularPart(), bodyTwist.getLinearPart(), linearAccelerationToPack);
linearJointTerm.setIncludingFrame(measurementFrame, 3, jointAccelerationTerm); linearConvectiveTerm.setIncludingFrame(measurementFrame, 3, convectiveTerm); sensorTwist.changeFrame(measurementFrame); 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);
checkReferenceFrameMatch(bodyTwist); linearAccelerationToPack.setIncludingFrame(bodyTwist.getLinearPart()); // v