private DenseMatrix64F getJointVelocityMatrix() { if (!isJointVelocityMatrixUpToDate) { List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocityMatrix); isJointVelocityMatrixUpToDate = true; } return jointVelocityMatrix; }
/** * Creates a new {@link JointMatrixIndexProvider} indexing all the given joints. * * @param jointsToIndex the array of joints to be indexed. Not modified. * @return the index provider for the given joints. */ public static JointMatrixIndexProvider toIndexProvider(JointReadOnly[] jointsToIndex) { return toIndexProvider(Arrays.asList(jointsToIndex)); }
private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); RecursionStep child = new RecursionStep(childBody, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); parent.children.add(child); buildMultiBodyTree(child, jointsToIgnore); } } }
private void buildMultiBodyTree(IterativeStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); IterativeStep child = new IterativeStep(childBody, jointIndices); rigidBodyToIterativeStepMap.put(childBody, child); parent.children.add(child); buildMultiBodyTree(child, jointsToIgnore); } } }
private DenseMatrix64F getJointAccelerationMatrix() { if (!isJointAccelerationMatrixUpToDate) { List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.ACCELERATION, jointAccelerationMatrix); isJointAccelerationMatrixUpToDate = true; } return jointAccelerationMatrix; }
/** * 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()); }
private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyInertia parent) { List<CompositeRigidBodyInertia> inertiaList = new ArrayList<>(); for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); CompositeRigidBodyInertia child = new CompositeRigidBodyInertia(childBody, parent, jointIndices); inertiaList.add(child); inertiaList.addAll(buildMultiBodyTree(child)); } } return inertiaList; }
private DenseMatrix64F getJointVelocityMatrix() { if (!isJointVelocityMatrixUpToDate) { List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocityMatrix); isJointVelocityMatrixUpToDate = true; } return jointVelocityMatrix; }
JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider);
private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); RecursionStep child = new RecursionStep(childBody, parent, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); buildMultiBodyTree(child, jointsToIgnore); } } }
private void updateCenterOfMassVelocity() { if (isCenterOfMassVelocityUpToDate) return; List<? extends JointReadOnly> joints = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(joints, JointStateType.VELOCITY, jointVelocityMatrix); CommonOps.mult(getJacobianMatrix(), jointVelocityMatrix, centerOfMassVelocityMatrix); centerOfMassVelocity.set(centerOfMassVelocityMatrix); isCenterOfMassVelocityUpToDate = true; }
JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider);
private void buildMultiBodyTree(ArticulatedBodyRecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); ArticulatedBodyRecursionStep child = new ArticulatedBodyRecursionStep(childBody, parent, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); buildMultiBodyTree(child, jointsToIgnore); } } }
public OriginalDynamicallyConsistentNullspaceCalculator(FloatingJointBasics rootJoint, boolean computeSNsBar) { this.rootJoint = rootJoint; this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); jointsInOrder = massMatrixCalculator.getInput().getJointMatrixIndexProvider().getIndexedJointsInOrder().toArray(new JointBasics[0]); this.nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); massMatrixSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); lambdaSolver = LinearSolverFactory.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); this.computeSNsBar = computeSNsBar; }
List<? extends JointBasics> jointsToIgnore = SubtreeStreams.fromChildren(rootBody).filter(joint -> !jointsToConsider.contains(joint)) .collect(Collectors.toList()); JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider);
private void buildMultiBodyTree(RecursionStep parent, Collection<? extends JointReadOnly> jointsToIgnore) { for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) { if (jointsToIgnore.contains(childJoint)) continue; RigidBodyReadOnly childBody = childJoint.getSuccessor(); if (childBody != null) { int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); RecursionStep child = new RecursionStep(childBody, parent, jointIndices); rigidBodyToRecursionStepMap.put(childBody, child); buildMultiBodyTree(child, jointsToIgnore); } } }
/** * Computes the joint efforts needed to achieve 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. */ public void compute(DenseMatrix64F jointAccelerationMatrix) { if (jointAccelerationMatrix != null) { this.jointAccelerationMatrix.set(jointAccelerationMatrix); } else { List<? extends JointReadOnly> indexedJointsInOrder = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(indexedJointsInOrder, JointStateType.ACCELERATION, this.jointAccelerationMatrix); } initialRecursionStep.passOne(); initialRecursionStep.passTwo(); }
List<? extends JointReadOnly> jointsToIgnore = SubtreeStreams.fromChildren(rootBody).filter(joint -> !jointsToConsider.contains(joint)) .collect(Collectors.toList()); JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider);
/** * Computes the joint accelerations resulting from the given joint efforts. * <p> * The given matrix is expected to have been configured using the same * {@link JointMatrixIndexProvider} that was used to configure this calculator. * </p> * * @param jointTauMatrix the matrix containing the joint efforts to use. Not modified. */ public void compute(DenseMatrix64F jointTauMatrix) { if (jointTauMatrix != null) { this.jointTauMatrix.set(jointTauMatrix); } else { List<? extends JointReadOnly> indexedJointsInOrder = input.getJointMatrixIndexProvider().getIndexedJointsInOrder(); MultiBodySystemTools.extractJointsState(indexedJointsInOrder, JointStateType.EFFORT, this.jointTauMatrix); } initialRecursionStep.passOne(); initialRecursionStep.passTwo(); initialRecursionStep.passThree(); }