public void sub(Wrench wrench) { putYoValuesIntoWrench(); this.wrench.sub(wrench); getYoValuesFromWrench(); }
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 cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = handCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempWristForce.setIncludingFrame(worldFrame, 0.0, 0.0, -handsMass.get(robotSide).getDoubleValue() * gravity); tempWristForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempWristForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = wristsubtreeCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempForce.setIncludingFrame(worldFrame, 0.0, 0.0, -wristSubtreeMass.get(robotSide).getDoubleValue() * gravity); tempForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }