How to use

calculate

method

in

us.ihmc.simulationconstructionset.simulatedSensors.WrenchCalculatorInterface

forceTorqueSensor.calculate();
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);
}

forceTorqueSensor.calculate();
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());
   }
 }
}

point1.getYoPosition().set(new Point3D(-1.0, 0.0, 0.0));
calculator.calculate();
point1.getYoPosition().set(new Point3D(-2.0, -2.0, 1.0));
calculator.calculate();
calculator.calculate();
wholeWrench = calculator.getWrench();
assertTrue(wholeWrench.getNumRows() == 6);
robot.update();
calculator.calculate();
wholeWrench = calculator.getWrench();
assertTrue(wholeWrench.getNumRows() == 6);

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