/** {@inheritDoc} */ @Override default List<? extends JointBasics> getJointsToIgnore() { return getAllJoints().stream().filter(joint -> !getJointsToConsider().contains(joint)).collect(Collectors.toList()); }
/** * Creates a new input from the given {@code rootBody}. The resulting input will consider all the * joints composing the subtree starting off the given body less the given joints to ignore and * their respective descendants. * * @param rootBody the support body to the subtree to consider. Not modified. * @param jointsToIgnore the array of joints to ignore. Not modified. * @return the new input. */ public static MultiBodySystemBasics toMultiBodySystemBasics(RigidBodyBasics rootBody, JointBasics[] jointsToIgnore) { return toMultiBodySystemBasics(rootBody, Arrays.asList(jointsToIgnore)); }
public ForwardDynamicComparisonScript(Robot robot, DRCRobotModel robotModel) { this.robot = robot; FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel(); List<JointBasics> ignoredJoints = robotModel.getJointMap().getLastSimulatedJoints().stream() .flatMap(name -> SubtreeStreams.fromChildren(fullRobotModel.getOneDoFJointByName(name).getSuccessor())) .collect(Collectors.toList()); multiBodySystem = MultiBodySystemBasics.toMultiBodySystemBasics(fullRobotModel.getElevator(), ignoredJoints); calculator = new ForwardDynamicsCalculator(multiBodySystem); floatingJoint = fullRobotModel.getRootJoint(); allOneDoFJoints = MultiBodySystemTools.filterJoints(multiBodySystem.getJointsToConsider(), OneDoFJointBasics.class); multiBodySystem.getAllJoints().forEach(joint -> nameToJointMap.put(joint.getName(), joint)); }
@Override public void doScript(double t) { try { robot.doDynamicsButDoNotIntegrate(); } catch (UnreasonableAccelerationException e) { e.printStackTrace(); } calculator.setGravitionalAcceleration(robot.getGravityZ()); getFloatingJointStateFromSCS(); getOneDoFJointStateFromSCS(); multiBodySystem.getRootBody().updateFramesRecursively(); getExternalWrenchesFromSCS(); calculator.compute(); calculator.writeComputedJointAccelerations(multiBodySystem.getJointsToConsider()); if (performAssertions) compareJointAccelerations(1.0e-5 * Math.max(1.0, findAccelerationGreatestMagnitude())); compareJointAccelerations(1.0e-3); }
/** {@inheritDoc} */ @Override default List<? extends JointBasics> getJointsToConsider() { return getAllJoints(); }
/** {@inheritDoc} */ @Override default List<? extends JointBasics> getAllJoints() { return SubtreeStreams.fromChildren(getRootBody()).collect(Collectors.toList()); }
List<? extends JointBasics> jointsToConsider = extractJointsToConsider(rootBody, jointsToIgnore); JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider);
/** * Creates a new input from the given {@code rootBody}. The resulting input will consider all the * joints composing the subtree starting off the given body. * * @param rootBody the support body to the subtree to consider. Not modified. * @return the new input. */ public static MultiBodySystemBasics toMultiBodySystemBasics(RigidBodyBasics rootBody) { return toMultiBodySystemBasics(rootBody, Collections.emptyList()); }
/** * Creates a new input from the given joints to consider. * <p> * The resulting root body and joints to ignore are automatically evaluated. * </p> * * @param jointsToConsider the joints to consider. Not modified. * @return the new input. */ public static MultiBodySystemBasics toMultiBodySystemBasics(JointBasics[] jointsToConsider) { return toMultiBodySystemBasics(Arrays.asList(jointsToConsider)); }
/** * 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); } }