private void updateMeasurement() { wrenchCalculatorInterface.calculate(); MatrixTools.extractFrameTupleFromEJMLVector(measuredForce, wrenchCalculatorInterface.getWrench(), measurementFrame, 3); measuredForceWorld.setIncludingFrame(measuredForce); measuredForceWorld.changeFrame(worldFrame); yoMeasuredForceWorld.setAndMatchFrame(measuredForce); }
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { OneDegreeOfFreedomJoint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJoint sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint(forceTorqueSensorJoint); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
public ForceSensorHysteresisCreator(double totalRobotMass, String parentJointName, WrenchCalculatorInterface wrenchCalculatorInterface) { super(wrenchCalculatorInterface.getName() + "HysteresisCreator"); this.wrenchCalculatorInterface = wrenchCalculatorInterface; this.wrenchCalculatorInterface.setDoWrenchCorruption(true); this.totalRobotWeightInNewtons = totalRobotMass * 9.81; this.normalForceThreshold = totalRobotWeightInNewtons * PERCENT_OF_FULL_WEIGHT_TO_TRIGGER_HYSTERESIS/100; hysteresisInZDirection = new YoDouble(parentJointName + "ForceSensorZHysteresis", registry); hysteresisInZDirection.set(0); this.isForcePastThresholdFiltered = new GlitchFilteredYoBoolean(parentJointName + "ForceSensorZHysteresisIsForcePastThreshold", registry, ITERS_BEFORE_HYSTERESIS_TRIGGERS); }
Joint joint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJointBasics oneDoFJoint; if (joint instanceof OneDegreeOfFreedomJoint) ReferenceFrame bodyFixedFrame = rigidBodyToApplyWrenchTo.getBodyFixedFrame(); groundContactPointBasedWrenchCalculator.calculate(); DenseMatrix64F wrenchFromSimulation = groundContactPointBasedWrenchCalculator.getWrench(); ReferenceFrame frameAtJoint = rigidBodyToApplyWrenchTo.getParentJoint().getFrameAfterJoint();
@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; } } }
public void packFootForceSensors(SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors) { ArrayList<WrenchCalculatorInterface> forceSensors = new ArrayList<WrenchCalculatorInterface>(); sdfRobot.getForceSensors(forceSensors); SideDependentList<String> jointNamesBeforeFeet = sdfRobot.getJointNamesBeforeFeet(); for (RobotSide robotSide : RobotSide.values) { footForceSensors.put(robotSide, new ArrayList<WrenchCalculatorInterface>()); for (int i = 0; i < forceSensors.size(); i++) { if (forceSensors.get(i).getJoint().getName().equals(jointNamesBeforeFeet.get(robotSide))) { footForceSensors.get(robotSide).add(forceSensors.get(i)); } } } }
public ArrayList<RobotController> getFootForceSensorHysteresisCreators() { SideDependentList<ArrayList<WrenchCalculatorInterface>> footForceSensors = new SideDependentList<ArrayList<WrenchCalculatorInterface>>(); packFootForceSensors(footForceSensors); ArrayList<RobotController> footForceSensorSignalCorruptors = new ArrayList<RobotController>(); for (RobotSide robotSide : RobotSide.values) { for (int i = 0; i < footForceSensors.get(robotSide).size(); i++) { ForceSensorHysteresisCreator forceSensorSignalCorruptor = new ForceSensorHysteresisCreator(sdfRobot.computeCenterOfMass(new Point3D()), footForceSensors.get(robotSide).get(i).getName(), footForceSensors.get(robotSide).get(i)); footForceSensorSignalCorruptors.add(forceSensorSignalCorruptor); } } return footForceSensorSignalCorruptors; }
private LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> generateForceSensorDefinitions( ArrayList<WrenchCalculatorInterface> groundContactPointBasedWrenchCalculators) { LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition> forceSensorDefinitions = new LinkedHashMap<WrenchCalculatorInterface, ForceSensorDefinition>(); for (WrenchCalculatorInterface groundContactPointBasedWrenchCalculator : groundContactPointBasedWrenchCalculators) { Joint forceTorqueSensorJoint = groundContactPointBasedWrenchCalculator.getJoint(); OneDoFJointBasics sensorParentJoint; if (forceTorqueSensorJoint instanceof OneDegreeOfFreedomJoint) sensorParentJoint = scsToInverseDynamicsJointMap.getInverseDynamicsOneDoFJoint((OneDegreeOfFreedomJoint) forceTorqueSensorJoint); else throw new RuntimeException("Force sensor is only supported for OneDegreeOfFreedomJoint."); RigidBodyTransform transformFromSensorToParentJoint = new RigidBodyTransform(); groundContactPointBasedWrenchCalculator.getTransformToParentJoint(transformFromSensorToParentJoint); ForceSensorDefinition sensorDefinition = new ForceSensorDefinition(groundContactPointBasedWrenchCalculator.getName(), sensorParentJoint.getSuccessor(), transformFromSensorToParentJoint); forceSensorDefinitions.put(groundContactPointBasedWrenchCalculator, sensorDefinition); } return forceSensorDefinitions; }
@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(); 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() { // 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(); DenseMatrix64F tauXFXAndFZ = calculator.getWrench(); assertEquals(1.0, tauXFXAndFZ.get(0, 0), epsilon); assertEquals(0.0, tauXFXAndFZ.get(3, 0), epsilon); point1.getYoPosition().set(new Point3D(-2.0, -2.0, 1.0)); calculator.calculate(); DenseMatrix64F wholeWrench = calculator.getWrench(); assertTrue(wholeWrench.getNumRows() == 6); assertEquals(wholeWrench.get(0,0), - 2.0, epsilon); calculator.calculate(); wholeWrench = calculator.getWrench(); assertTrue(wholeWrench.getNumRows() == 6); assertEquals(wholeWrench.get(0,0), - 2.0, epsilon); robot.update(); calculator.calculate(); wholeWrench = calculator.getWrench(); assertTrue(wholeWrench.getNumRows() == 6); assertEquals(wholeWrench.get(0,0), - 2.0, epsilon);