RigidBodyBasics rootBody = (RigidBodyBasics) MultiBodySystemReadOnly.getClosestJointToRoot(jointsToConsider).getPredecessor(); List<? extends JointBasics> allJoints = SubtreeStreams.fromChildren(rootBody).collect(Collectors.toList()); List<? extends JointBasics> jointsToIgnore = SubtreeStreams.fromChildren(rootBody).filter(joint -> !jointsToConsider.contains(joint))
RigidBodyReadOnly rootBody = getClosestJointToRoot(jointsToConsider).getPredecessor(); List<? extends JointReadOnly> allJoints = SubtreeStreams.fromChildren(rootBody).collect(Collectors.toList()); List<? extends JointReadOnly> jointsToIgnore = SubtreeStreams.fromChildren(rootBody).filter(joint -> !jointsToConsider.contains(joint))