private Wrench createEmptyWrench() { return new Wrench(bodyFrame, expressedInFrame); }
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 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; }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; setMotionSubspace(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); this.successorWrench = new Wrench(successorFrame, successorFrame); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; setMotionSubspace(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); this.successorWrench = new Wrench(successorFrame, successorFrame); }
public void setExternalWrenchToCompensateFor(RigidBody rigidBody, Wrench wrench) { boolean containsRigidBody = externalWrenchesToCompensateFor.get(rigidBody) != null; if (!containsRigidBody) { externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); externalWrenchesToCompensateFor.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); externalWrenchesToCompensateForList.add(externalWrenchesToCompensateFor.get(rigidBody)); rigidBodiesWithWrenchToCompensateFor.add(rigidBody); } ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrench.getBodyFrame().checkReferenceFrameMatch(bodyFixedFrame); wrench.getExpressedInFrame().checkReferenceFrameMatch(bodyFixedFrame); externalWrenchesToCompensateFor.get(rigidBody).set(wrench); }
footForcesZInPercentOfTotalForce.put(foot, footForceZInPercentOfTotalForce); footWrenches.put(foot, new Wrench());
public ExternalWrenchHandler(double gravityZ, ReferenceFrame centerOfMassFrame, InverseDynamicsJoint rootJoint, List<? extends ContactablePlaneBody> contactablePlaneBodies) { this.centerOfMassFrame = centerOfMassFrame; MathTools.checkIfInRange(gravityZ, 0.0, Double.POSITIVE_INFINITY); this.contactablePlaneBodies = new ArrayList<>(contactablePlaneBodies); gravitationalWrench = new SpatialForceVector(centerOfMassFrame); double totalMass = TotalMassCalculator.computeMass(ScrewTools.computeSupportAndSubtreeSuccessors(rootJoint.getSuccessor())); gravitationalWrench.setLinearPartZ(-gravityZ * totalMass); totalWrenchAlreadyApplied = new SpatialForceVector(centerOfMassFrame); for (int i = 0; i < contactablePlaneBodies.size(); i++) { RigidBody rigidBody = this.contactablePlaneBodies.get(i).getRigidBody(); externalWrenches.put(rigidBody, new Wrench(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame())); } }
public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { twistCalculator = new TwistCalculator(inertialFrame, rootBody); LinkedHashMap<RigidBody, Wrench> zeroExternalWrench = new LinkedHashMap<RigidBody, Wrench>(); ArrayList<InverseDynamicsJoint> zeroJointToIgnore = new ArrayList<InverseDynamicsJoint>(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(inertialFrame, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true, twistCalculator); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); for (InverseDynamicsJoint joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); storedJointWrenches.put(joint, jointWrench); } }
Wrench wrench = new Wrench(bodyFixedFrame, bodyFixedFrame); wrenchesFromRho.put(rigidBody, wrench);
wristSensorWrench = new Wrench(); forceSensorData.getWrench(wristSensorWrench);
public ForceSensorDistalMassCompensator(ForceSensorDefinition forceSensorDefinition, double dtForLowpassFilter, YoVariableRegistry registry) { String sensorName = forceSensorDefinition.getSensorName(); InverseDynamicsJoint parentJointOfSensorBody = forceSensorDefinition.getRigidBody().getParentJoint(); sensorFrame = forceSensorDefinition.getSensorFrame(); sensorPose = new FramePose(world); yoSensorPositionInWorld = new YoFramePoint(sensorName + "Position", world, registry); distalMassCalc = new CenterOfMassCalculator(ScrewTools.computeRigidBodiesAfterThisJoint(parentJointOfSensorBody), world); distalMass = new DoubleYoVariable(sensorName + "DistalMass", registry); lowPassSensorForceZ = new AlphaFilteredYoVariable(sensorName + "LowPassFz", registry, AlphaFilteredYoVariable.computeAlphaGivenBreakFrequencyProperly(0.0001, dtForLowpassFilter)); distalMassForceInWorld = new YoFrameVector(sensorName + "DistalWeight", world, registry); distalCoMInWorld = new YoFramePoint(sensorName + "DistalCoM", world, registry); yoSensorToDistalCoMvectorInWorld = new YoFrameVector(sensorName + "ToDistalCoM", world, registry); distalMassWrench = new Wrench(sensorFrame, world); // Put sensor values in world frame since it's easy to interpret from looking at GUI yoSensorForce = new YoFrameVector(sensorName + "Force", world, registry); yoSensorTorque = new YoFrameVector(sensorName + "Torque", world, registry); yoSensorForceFromDistalMass = new YoFrameVector(sensorName + "ForceDueToDistalMass", world, registry); yoSensorTorqueFromDistalMass = new YoFrameVector(sensorName + "TorqueDueToDistalMass", world, registry); yoSensorForceMassCompensated = new YoFrameVector(sensorName + "ForceMassCompensated", world, registry); yoSensorTorqueMassCompensated = new YoFrameVector(sensorName + "TorqueMassCompensated", world, registry); addSimulatedSensorNoise = new BooleanYoVariable(sensorName + "AddSimulatedNoise", registry); addSimulatedSensorNoise.set(false); }
wrenches.put(measurementLink, new Wrench());
if (hand != null) handWrenches.put(robotSide, new Wrench());
this.footSwitchCoPThresholdFraction.set(footSwitchCoPThresholdFraction); this.footWrench = new Wrench(forceSensorData.getMeasurementFrame(), null);
Wrench footWrench = new Wrench(); ForceSensorDataReadOnly forceSensorData = ankleForceSensors.get(robotSide); ReferenceFrame measurementFrame = forceSensorData.getMeasurementFrame();