/** * Creates a new calculator for the given {@code input}. * * @param input the definition of the system to be evaluated by this calculator. */ public CompositeRigidBodyMassMatrixCalculator(MultiBodySystemReadOnly input) { this(input, input.getRootBody().getBodyFixedFrame()); }
/** * Gets all the joints composing the subtree starting at {@link #getRootBody()} including both the * joints to consider and to ignore. * * @return the list containing all the joints. */ default List<? extends JointReadOnly> getAllJoints() { return SubtreeStreams.fromChildren(getRootBody()).collect(Collectors.toList()); }
/** * Performs a deep copy of {@code original}, preserving naming, root body, and the joints to ignore. * The clone is attached to the given {@code clonerootFrame}. * * @param original the multi-body system to clone. Not modified. * @param cloneRootFrame the root frame to which the clone system is attached. * @return the clone. */ public static MultiBodySystemBasics clone(MultiBodySystemReadOnly original, ReferenceFrame cloneRootFrame) { RigidBodyBasics cloneRootBody = MultiBodySystemFactories.cloneMultiBodySystem(original.getRootBody(), cloneRootFrame, ""); Set<String> namesOfJointsToConsider = SubtreeStreams.fromChildren(original.getRootBody()).map(JointReadOnly::getName).collect(Collectors.toSet()); List<? extends JointBasics> jointsToConsider = SubtreeStreams.fromChildren(cloneRootBody) .filter(joint -> namesOfJointsToConsider.contains(joint.getName())) .collect(Collectors.toList()); return toMultiBodySystemBasics(jointsToConsider); } }
/** * Gets the inertial frame to use with this multi-body system. * <p> * The inertial frame is used as the principal referential when measuring the motion of an object in * space. It is usually set to the root frame of the reference frame tree this system is attached * to. * </p> * * @return the inertial frame. */ default ReferenceFrame getInertialFrame() { return getRootBody().getBodyFixedFrame().getRootFrame(); }
/** * Creates a new calculator for the given {@code input}. * * @param input the definition of the system to be evaluated by this calculator. * @param centroidalMomentumFrame the reference frame in which the centroidal momentum matrix and * convective term are to be computed. Only needed when this calculator is used to * calculate the centroidal momentum matrix and convective term. * @param considerIgnoredSubtreesInertia whether the inertia of the ignored part(s) of the given * multi-body system should be considered. When {@code true}, this provides a more * accurate mass matrix, while when {@code false}, this calculator may gain slight * performance improvement. */ public CompositeRigidBodyMassMatrixCalculator(MultiBodySystemReadOnly input, ReferenceFrame centroidalMomentumFrame, boolean considerIgnoredSubtreesInertia) { this.input = input; RigidBodyReadOnly rootBody = input.getRootBody(); rootCompositeInertia = new CompositeRigidBodyInertia(rootBody, null, null); List<CompositeRigidBodyInertia> inertiaList = buildMultiBodyTree(rootCompositeInertia); if (considerIgnoredSubtreesInertia) rootCompositeInertia.includeIgnoredSubtreeInertia(); compositeInertias = inertiaList.toArray(new CompositeRigidBodyInertia[0]); int nDoFs = SubtreeStreams.from(rootBody.getChildrenJoints()).mapToInt(JointReadOnly::getDegreesOfFreedom).sum(); massMatrix = new DenseMatrix64F(nDoFs, nDoFs); centroidalMomentumMatrix = new DenseMatrix64F(6, nDoFs); setCentroidalMomentumFrame(centroidalMomentumFrame); }
RigidBodyReadOnly rootBody = input.getRootBody(); rootFrame = rootBody.getBodyFixedFrame().getRootFrame(); if (isJacobianFrameAtCenterOfMass)
RigidBodyReadOnly rootBody = input.getRootBody(); initialRecursionStep = new ArticulatedBodyRecursionStep(rootBody, null, null); rigidBodyToRecursionStepMap.put(rootBody, initialRecursionStep);
this.considerCoriolisAndCentrifugalForces = considerCoriolisAndCentrifugalForces; RigidBodyReadOnly rootBody = input.getRootBody(); initialRecursionStep = new RecursionStep(rootBody, null, null); rigidBodyToRecursionStepMap.put(rootBody, initialRecursionStep);
this.matrixFrame = matrixFrame; RigidBodyReadOnly rootBody = input.getRootBody(); IterativeStep initialIterativeStep = new IterativeStep(rootBody, null); rigidBodyToIterativeStepMap.put(rootBody, initialIterativeStep);
this.matrixFrame = matrixFrame; RigidBodyReadOnly rootBody = input.getRootBody(); initialRecursionStep = new RecursionStep(rootBody, null, null); rigidBodyToRecursionStepMap.put(rootBody, initialRecursionStep);