public static void addExternalWrenches(Map<RigidBody, Wrench> externalWrenches, Map<RigidBody, Wrench> wrenchMapToAdd) { for (RigidBody rigidBody : wrenchMapToAdd.keySet()) { Wrench externalWrenchToCompensateFor = wrenchMapToAdd.get(rigidBody); Wrench externalWrench = externalWrenches.get(rigidBody); if (externalWrench == null) { externalWrenches.put(rigidBody, new Wrench(externalWrenchToCompensateFor)); } else { externalWrench.add(externalWrenchToCompensateFor); } } }
public void computeExternalWrenches(Map<RigidBody, Wrench> groundReactionWrenches) { for (int i = 0; i < contactablePlaneBodies.size(); i++) { RigidBody rigidBody = contactablePlaneBodies.get(i).getRigidBody(); Wrench externalWrench = externalWrenches.get(rigidBody); if(groundReactionWrenches.containsKey(rigidBody)) externalWrench.add(groundReactionWrenches.get(rigidBody)); if (!rigidBodiesWithExternalWrench.contains(rigidBody)) rigidBodiesWithExternalWrench.add(rigidBody); } for (int i = 0; i < rigidBodiesWithWrenchToCompensateFor.size(); i++) { RigidBody rigidBody = rigidBodiesWithWrenchToCompensateFor.get(i); Wrench externalWrench = externalWrenches.get(rigidBody); externalWrench.add(externalWrenchesToCompensateFor.get(rigidBody)); if (!rigidBodiesWithExternalWrench.contains(rigidBody)) rigidBodiesWithExternalWrench.add(rigidBody); } }
public void add(Wrench wrench) { putYoValuesIntoWrench(); this.wrench.add(wrench); getYoValuesFromWrench(); }
public Wrench computeTotalExternalWrench(ReferenceFrame referenceFrame) { Wrench totalGroundReactionWrench = new Wrench(referenceFrame, referenceFrame); Wrench temporaryWrench = new Wrench(); for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) { Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); temporaryWrench.set(externalWrench); temporaryWrench.changeFrame(referenceFrame); temporaryWrench.changeBodyFrameAttachedToSameBody(referenceFrame); totalGroundReactionWrench.add(temporaryWrench); } return totalGroundReactionWrench; }
public Map<RigidBody, Wrench> computeWrenchesFromRho(DenseMatrix64F rho) { // Reinintialize wrenches for (int i = 0; i < rigidBodies.size(); i++) { RigidBody rigidBody = rigidBodies.get(i); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchesFromRho.get(rigidBody).setToZero(bodyFixedFrame, bodyFixedFrame); } int rhoStartIndex = 0; for (int i = 0; i < rigidBodies.size(); i++) { RigidBody rigidBody = rigidBodies.get(i); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); PlaneContactStateToWrenchMatrixHelper helper = planeContactStateToWrenchMatrixHelpers.get(rigidBody); helper.computeWrenchFromRho(rhoStartIndex, rho); Wrench wrenchFromRho = helper.getWrenchFromRho(); wrenchFromRho.changeFrame(bodyFixedFrame); wrenchesFromRho.get(rigidBody).add(wrenchFromRho); rhoStartIndex += helper.getRhoSize(); } return wrenchesFromRho; }
public void computeTotalWrench(Wrench totalGroundReactionWrenchToPack, Collection<Wrench> wrenches, ReferenceFrame referenceFrame) { totalGroundReactionWrenchToPack.setToZero(referenceFrame, referenceFrame); for (Wrench wrench : wrenches) { temporaryWrench.set(wrench); temporaryWrench.changeFrame(referenceFrame); temporaryWrench.changeBodyFrameAttachedToSameBody(referenceFrame); totalGroundReactionWrenchToPack.add(temporaryWrench); } } }