@Override public void getWrench(Wrench wrenchToPack) { wrenchToPack.changeBodyFrameAttachedToSameBody(measurementFrame); wrenchToPack.set(measurementFrame, wrench); }
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 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); } } }
private void computeJointWrenchesAndTorques() { for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) { InverseDynamicsJoint joint = allJoints.get(jointIndex); RigidBody successor = joint.getSuccessor(); Wrench jointWrench = jointWrenches.get(joint); jointWrench.set(netWrenches.get(successor)); Wrench externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); List<InverseDynamicsJoint> childrenJoints = successor.getChildrenJoints(); for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) { InverseDynamicsJoint child = childrenJoints.get(childIndex); if (!jointsToIgnore.contains(child)) { Wrench wrenchExertedOnChild = jointWrenches.get(child); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); wrenchExertedByChild.set(wrenchExertedOnChild); wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame); wrenchExertedByChild.scale(-1.0); // Action = -reaction wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame()); jointWrench.sub(wrenchExertedByChild); } } joint.setTorqueFromWrench(jointWrench); } }
private void setFootMeasuredWrenches() { for (RobotSide robotSide : RobotSide.values) { WrenchBasedFootSwitch wrenchBasedFootSwitch = wrenchBasedFootSwitches.get(robotSide); wrenchBasedFootSwitch.computeAndPackFootWrench(tempWrench); RigidBody foot = wrenchBasedFootSwitch.getContactablePlaneBody().getRigidBody(); tempWrench.changeBodyFrameAttachedToSameBody(foot.getBodyFixedFrame()); tempWrench.changeFrame(foot.getBodyFixedFrame()); inverseDynamicsCalculator.setExternalWrench(foot, tempWrench); } }
wrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); groundReactionWrenches.put(controlledBody, wrench);
externalWrenchSolution.get(rigidBody).changeBodyFrameAttachedToSameBody(rigidBody.getBodyFixedFrame()); externalWrenchSolution.get(rigidBody).negate();
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()); }
public void control(SpatialAccelerationVector handSpatialAccelerationVector, Wrench toolWrench) { if (!hasBeenInitialized) { update(); initialize(); } update(); toolAcceleration.set(handSpatialAccelerationVector); toolAcceleration.changeFrameNoRelativeMotion(toolJoint.getFrameAfterJoint()); // TODO: Take relative acceleration between uTorsoCoM and elevator in account toolAcceleration.changeBaseFrameNoRelativeAcceleration(elevatorFrame); toolAcceleration.changeBodyFrameNoRelativeAcceleration(toolJoint.getFrameAfterJoint()); toolJoint.setDesiredAcceleration(toolAcceleration); inverseDynamicsCalculator.compute(); inverseDynamicsCalculator.getJointWrench(toolJoint, toolWrench); toolWrench.negate(); toolWrench.changeFrame(handFixedFrame); toolWrench.changeBodyFrameAttachedToSameBody(handFixedFrame); // Visualization temporaryVector.setIncludingFrame(handFixedFrame, toolWrench.getLinearPartX(), toolWrench.getLinearPartY(), toolWrench.getLinearPartZ()); temporaryVector.changeFrame(ReferenceFrame.getWorldFrame()); temporaryVector.scale(0.01); objectForceInWorld.set(temporaryVector); }
tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame());
wrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); wrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame());