/** * Creates a new calculator for the given {@code input}. * * @param input the definition of the system to be evaluated by this calculator. * @param referenceFrame the frame in which the center of mass is to be expressed. */ public CenterOfMassCalculator(MultiBodySystemReadOnly input, ReferenceFrame referenceFrame) { this.input = input; rigidBodies = input.getJointsToConsider().stream().map(JointReadOnly::getSuccessor).toArray(RigidBodyReadOnly[]::new); this.referenceFrame = referenceFrame; }
@Override public JointBasics[] getJointsInOrder() { return compositeMassMatricCalculator.getInput().getJointsToConsider().toArray(new JointBasics[0]); }
/** * Gets all the joints to be ignored in the algorithm. * * @return the list of all the joints to be ignored. */ default List<? extends JointReadOnly> getJointsToIgnore() { return getAllJoints().stream().filter(joint -> !getJointsToConsider().contains(joint)).collect(Collectors.toList()); }
@Override public JointBasics[] getJointsInOrder() { return crbmmc.getInput().getJointsToConsider().toArray(new JointBasics[0]); }
/** * Gets the {@code JointMatrixIndexProvider} to use with this input. * * @return the matrix index provider for the considered joints. */ default JointMatrixIndexProvider getJointMatrixIndexProvider() { return JointMatrixIndexProvider.toIndexProvider(getJointsToConsider()); }
/** * Gets the total mass of the multi-body system. * * @return the mass of the multi-body system. */ public double getTotalMass() { if (!isTotalMassUpToDate) { totalMass = 0.0; List<? extends JointReadOnly> jointsToConsider = input.getJointsToConsider(); for (int i = 0; i < jointsToConsider.size(); i++) { totalMass += jointsToConsider.get(i).getSuccessor().getInertia().getMass(); } isTotalMassUpToDate = true; } return totalMass; }
/** * Gets the total mass of the multi-body system. * * @return the mass of the multi-body system. */ public double getTotalMass() { if (!isTotalMassUpToDate) { totalMass = 0.0; List<? extends JointReadOnly> jointsToConsider = input.getJointsToConsider(); for (int i = 0; i < jointsToConsider.size(); i++) { totalMass += jointsToConsider.get(i).getSuccessor().getInertia().getMass(); } isTotalMassUpToDate = true; } return totalMass; }
int nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); jacobianMatrix = new DenseMatrix64F(3, nDegreesOfFreedom); jointVelocityMatrix = new DenseMatrix64F(nDegreesOfFreedom, 1);
initialRecursionStep.includeIgnoredSubtreeInertia(); int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); jointTauMatrix = new DenseMatrix64F(nDoFs, 1); jointAccelerationMatrix = new DenseMatrix64F(nDoFs, 1);
initialRecursionStep.includeIgnoredSubtreeInertia(); int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); jointAccelerationMatrix = new DenseMatrix64F(nDoFs, 1); jointTauMatrix = new DenseMatrix64F(nDoFs, 1);
centerOfMassVelocity = new FrameVector3D(matrixFrame); int nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom); jointVelocityMatrix = new DenseMatrix64F(nDegreesOfFreedom, 1);
netCoriolisBodyWrench = new Wrench(); int nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); centroidalMomentumMatrix = new DenseMatrix64F(6, nDegreesOfFreedom); jointVelocityMatrix = new DenseMatrix64F(nDegreesOfFreedom, 1);