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 computeADotVRightSide() { // Here we calculate Jdot * v by computing the spatial acceleration with vddot = 0 for(int j = 0; j<jointList.length; j++) { spatialAccelerationCalculator.getAccelerationOfBody(tempSpatialAcceleration, rigidBodies[j]); tempSpatialAcceleration.getMatrix(tempSpatialMotionMatrix, 0); CommonOps.mult(denseAdjTimesI[j], tempSpatialMotionMatrix, tempMatrix); CommonOps.add(aDotV, tempMatrix, aDotV); } }
getAccelerationOfBody(baseAcceleration, base); getAccelerationOfBody(spatialAccelerationToPack, body);
twistCalculator.getTwistOfBody(twistOfBodyWithRespectToBase, body); getAccelerationOfBody(baseAcceleration, base); getAccelerationOfBody(endEffectorAcceleration, body);
spatialAccelerationCalculator.getAccelerationOfBody(tempSpatialAcceleration, rigidBody);