private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { RigidBody body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); twistCalculator.getTwistOfBody(tempTwist, body); if (!doVelocityTerms) tempTwist.setToZero(); spatialAccelerationCalculator.getAccelerationOfBody(tempAcceleration, body); body.getInertia().computeDynamicWrenchInBodyCoordinates(tempAcceleration, tempTwist, netWrench); } }
private void handleSpatialAccelerationCommand(SpatialAccelerationCommand command) { RigidBody controlledBody = command.getEndEffector(); SpatialAccelerationVector accelerationVector = command.getSpatialAcceleration(); accelerationVector.changeBaseFrameNoRelativeAcceleration(ReferenceFrame.getWorldFrame()); twistCalculator.getTwistOfBody(tmpTwist, controlledBody); tmpWrench.setToZero(accelerationVector.getBodyFrame(), accelerationVector.getExpressedInFrame()); conversionInertias.get(controlledBody).computeDynamicWrenchInBodyCoordinates(accelerationVector, tmpTwist, tmpWrench); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(controlledBody, tmpWrench, command.getSelectionMatrix()); virtualWrenchCommandList.addCommand(virtualWrenchCommand); if (controlledBody == controlRootBody) { tmpExternalWrench.set(tmpWrench); tmpExternalWrench.negate(); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }