/** * 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()); }