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();