public void sub(Twist twist) { putYoValuesIntoTwist(); this.twist.sub(twist); getYoValuesFromTwist(); }
tempTwist.set(computeOrGetTwistOfBody(base)); tempTwist.changeFrame(twistToPack.getExpressedInFrame()); twistToPack.sub(tempTwist);
private void computeADotVLeftSide() { for(int i = 0; i < bodyTwists.length; i++) { tempTwist.set(comTwist); tempTwist.sub(bodyTwists[i]); // Left multiply by ad^{T} : this is separated out rather than creating matrix objects to save computation time. // See method below for creating adjoint from twist for reference. // ad^{T} * Ad^{T} * I * J * v // The order of these cross products are backwards because I need -a x b and cross products are anticommutative, // so -a x b = b x a. leftSide.setToZero(); tempVector.cross(bodyMomenta[i].getLinearPart(), tempTwist.getLinearPart()); leftSide.addAngularPart(tempVector); tempVector.cross(bodyMomenta[i].getAngularPart(), tempTwist.getAngularPart()); leftSide.addAngularPart(tempVector); tempVector.cross(bodyMomenta[i].getLinearPart(), tempTwist.getAngularPart()); leftSide.addLinearPart(tempVector); // leftSide.getMatrix(tempSpatialForceMatrix); CommonOps.add(aDotV, tempSpatialForceMatrix, aDotV); } }
tempCoMTwist.sub(tempTwist); //Twist of the CoM w.r.t. body i expressed in CoM frame. tempCoMTwist.changeFrame(centerOfMassFrame);