How to use

getWrench

method

in

us.ihmc.simulationconstructionset.simulatedSensors.WrenchCalculatorInterface

sensorProcessing.setForceSensorValue(forceTorqueSensors.get(i).getLeft(), forceTorqueSensor.getWrench());

private void updateMeasurement()
{
 wrenchCalculatorInterface.calculate();
 MatrixTools.extractFrameTupleFromEJMLVector(measuredForce, wrenchCalculatorInterface.getWrench(), measurementFrame, 3);
 measuredForceWorld.setIncludingFrame(measuredForce);
 measuredForceWorld.changeFrame(worldFrame);
 yoMeasuredForceWorld.setAndMatchFrame(measuredForce);
}

sensorProcessing.setForceSensorValue(forceTorqueSensors.get(i).getLeft(), forceTorqueSensor.getWrench());

@Override
public void read()
{
 // Think about adding root body acceleration to the fullrobotmodel
 readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations();
 readAndUpdateRootJointPositionAndOrientation();
 readAndUpdateRootJointAngularAndLinearVelocity();
 // Update frames after setting angular and linear velocities to correctly update zup frames
 updateReferenceFrames();
 long timestamp = Conversions.secondsToNanoseconds(robot.getTime());
 this.timestamp.set(timestamp);
 this.visionSensorTimestamp.set(timestamp);
 this.sensorHeadPPSTimetamp.set(timestamp);
 if (forceSensorDataHolderToUpdate != null)
 {
   for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet())
   {
    final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue();
    forceTorqueSensor.calculate();
    forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench());
   }
 }
}

@Override
public void read()
{
 readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations();
 readAndUpdateRootJointPositionAndOrientation();
 readAndUpdateRootJointAngularAndLinearVelocity();
 readAndUpdateIMUSensors();
 long timestamp = Conversions.secondsToNanoseconds(robot.getTime());
 sensorOutputMap.setTimestamp(timestamp);
 sensorOutputMap.setVisionSensorTimestamp(timestamp);
 sensorOutputMap.setSensorHeadPPSTimetamp(timestamp);
 
 if (forceSensorDataHolderToUpdate != null)
 {
   for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet())
   {
    final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue();
    forceTorqueSensor.calculate();
    forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench());
   }
 }
}

@Override
public void read()
{
 // Think about adding root body acceleration to the fullrobotmodel
 readAndUpdateOneDoFJointPositionsVelocitiesAndAccelerations();
 readAndUpdateRootJointPositionAndOrientation();
 updateReferenceFrames();
 readAndUpdateRootJointAngularAndLinearVelocity();
 long timestamp = TimeTools.secondsToNanoSeconds(robot.getTime());
 this.timestamp.set(timestamp);
 this.visionSensorTimestamp.set(timestamp);
 this.sensorHeadPPSTimetamp.set(timestamp);
 if (forceSensorDataHolderToUpdate != null)
 {
   for (Entry<ForceSensorDefinition, WrenchCalculatorInterface> forceTorqueSensorEntry : forceTorqueSensors.entrySet())
   {
    final WrenchCalculatorInterface forceTorqueSensor = forceTorqueSensorEntry.getValue();
    forceTorqueSensor.calculate();
    forceSensorDataHolderToUpdate.setForceSensorValue(forceTorqueSensorEntry.getKey(), forceTorqueSensor.getWrench());
   }
 }
}

@Override
public void doControl()
{
 super.doControl();
 
 wrench = wrenchCalculatorInterface.getWrench();
 
 isNormalForcePastHysteresisThreshold();
 
 if(isForcePastThresholdFiltered.getBooleanValue() && unfilteredIsForcePastThresh) //take lots of true readings to turn on, only 1 to turn off
 {  
   hysteresisSampleCounter++;
   tmpHysteresis += wrench.get(Wrench.SIZE-1);
   hasNormalForceGonePastLimit = true;
 }
 else
 {
   if(hasNormalForceGonePastLimit)
   {
    hysteresisInZDirection.set(hysteresisInZDirection.getDoubleValue() + (tmpHysteresis/hysteresisSampleCounter)*HYSTERESIS_PERCENT_OF_LOAD/100);
    wrenchCalculatorInterface.corruptWrenchElement(Wrench.SIZE-1,hysteresisInZDirection.getDoubleValue());
    
    tmpHysteresis = 0;
    hysteresisSampleCounter = 0;
    hasNormalForceGonePastLimit = false;
   }
 } 
}

DenseMatrix64F tauXFXAndFZ = calculator.getWrench();
assertEquals(1.0, tauXFXAndFZ.get(0, 0), epsilon);
assertEquals(0.0, tauXFXAndFZ.get(3, 0), epsilon);
DenseMatrix64F wholeWrench = calculator.getWrench();
assertTrue(wholeWrench.getNumRows() == 6);
assertEquals(wholeWrench.get(0,0), - 2.0, epsilon);
wholeWrench = calculator.getWrench();
assertTrue(wholeWrench.getNumRows() == 6);
assertEquals(wholeWrench.get(0,0), - 2.0, epsilon);
wholeWrench = calculator.getWrench();
assertTrue(wholeWrench.getNumRows() == 6);
assertEquals(wholeWrench.get(0,0), - 2.0, epsilon);

DenseMatrix64F wrenchFromSimulation = groundContactPointBasedWrenchCalculator.getWrench();
ReferenceFrame frameAtJoint = rigidBodyToApplyWrenchTo.getParentJoint().getFrameAfterJoint();