private int getForceSensorIndex(String forceSensorName, ForceSensorDefinition[] forceSensorDefinitions) { for(int i = 0; i < forceSensorDefinitions.length; i++) { if(forceSensorDefinitions[i].getSensorName().equals(forceSensorName)) { return i; } } return -1; }
private int getForceSensorIndex(String forceSensorName, ForceSensorDefinition[] forceSensorDefinitions) { for(int i = 0; i < forceSensorDefinitions.length; i++) { if(forceSensorDefinitions[i].getSensorName().equals(forceSensorName)) { return i; } } return -1; }
private int getForceSensorIndex(String forceSensorName, ForceSensorDefinition[] forceSensorDefinitions) { for (int i = 0; i < forceSensorDefinitions.length; i++) { if (forceSensorDefinitions[i].getSensorName().equals(forceSensorName)) { return i; } } return -1; }
@Override public boolean equals(Object other) { if(other instanceof ForceSensorDefinition) { ForceSensorDefinition otherSensor = (ForceSensorDefinition) other; return otherSensor.getSensorName().equals(getSensorName()) && otherSensor.getParentJointName().equals(getParentJointName()); } return false; } }
public ForceSensorDataHolder(List<ForceSensorDefinition> forceSensors) { for (ForceSensorDefinition forceSensorDefinition : forceSensors) { ForceSensorData forceSensor = new ForceSensorData(forceSensorDefinition); forceSensorDefinitions.add(forceSensorDefinition); this.forceSensors.put(forceSensorDefinition, forceSensor); sensorNameToDefintionMap.put(forceSensorDefinition.getSensorName(), forceSensorDefinition); } }
@Override public boolean equals(Object other) { if(other instanceof ForceSensorDefinition) { ForceSensorDefinition otherSensor = (ForceSensorDefinition) other; return otherSensor.getSensorName().equals(getSensorName()) && otherSensor.getParentJointName().equals(getParentJointName()); } return false; } }
public ForceSensorDataHolder(List<ForceSensorDefinition> forceSensors) { for (ForceSensorDefinition forceSensorDefinition : forceSensors) { ForceSensorData forceSensor = new ForceSensorData(forceSensorDefinition); forceSensorDefinitions.add(forceSensorDefinition); this.forceSensors.put(forceSensorDefinition, forceSensor); sensorNameToDefintionMap.put(forceSensorDefinition.getSensorName(), forceSensorDefinition); } }
public static int calculateJointNameHash(OneDoFJointBasics[] joints, ForceSensorDefinition[] forceSensorDefinitions, IMUDefinition[] imuDefinitions) { CRC32 crc = new CRC32(); for (OneDoFJointBasics joint : joints) { crc.update(joint.getName().getBytes()); } for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) { crc.update(forceSensorDefinition.getSensorName().getBytes()); } for (IMUDefinition imuDefinition : imuDefinitions) { crc.update(imuDefinition.getName().getBytes()); } return (int) crc.getValue(); }
public static int calculateJointNameHash(OneDoFJoint[] joints, ForceSensorDefinition[] forceSensorDefinitions, IMUDefinition[] imuDefinitions) { CRC32 crc = new CRC32(); for (OneDoFJoint joint : joints) { crc.update(joint.getName().getBytes()); } for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) { crc.update(forceSensorDefinition.getSensorName().getBytes()); } for (IMUDefinition imuDefinition : imuDefinitions) { crc.update(imuDefinition.getName().getBytes()); } return (int) crc.getValue(); }
public void set(ForceSensorDataHolderReadOnly otherForceSensorDataHolder) { for (int i = 0; i < forceSensorDefinitions.size(); i++) { final ForceSensorDefinition forceSensorDefinition = forceSensorDefinitions.get(i); ForceSensorDataReadOnly otherForceSensorData = otherForceSensorDataHolder.get(forceSensorDefinition); if (otherForceSensorData == null) { if (firstException) { firstException = false; System.err.println(getClass().getSimpleName() + " Could not find the force sensor: " + forceSensorDefinition.getSensorName()); } } else { otherForceSensorData.getWrench(tempWrench); forceSensors.get(forceSensorDefinition).setWrench(tempWrench); } } }
public void set(ForceSensorDataHolderReadOnly otherForceSensorDataHolder) { for (int i = 0; i < forceSensorDefinitions.size(); i++) { final ForceSensorDefinition forceSensorDefinition = forceSensorDefinitions.get(i); ForceSensorDataReadOnly otherForceSensorData = otherForceSensorDataHolder.get(forceSensorDefinition); if (otherForceSensorData == null) { if (firstException) { firstException = false; System.err.println(getClass().getSimpleName() + " Could not find the force sensor: " + forceSensorDefinition.getSensorName()); } } else { otherForceSensorData.getWrench(tempWrench); forceSensors.get(forceSensorDefinition).setWrench(tempWrench); } } }
private Map<String, Integer> addForceSensorAlphaFilterWithSensorsToIgnore(DoubleYoVariable alphaFilter, boolean forVizOnly, SensorType sensorType, List<String> sensorsToIgnore) { Map<String, Integer> processorsIDs = new HashMap<>(); LinkedHashMap<ForceSensorDefinition, YoFrameVector> intermediateForceSensorSignals = getIntermediateForceSensorSignals(sensorType); LinkedHashMap<ForceSensorDefinition, List<ProcessingYoVariable>> processedForceSensorSignals = getProcessedForceSensorSignals(sensorType); for (int i = 0; i < forceSensorDefinitions.size(); i++) { ForceSensorDefinition forceSensorDefinition = forceSensorDefinitions.get(i); String sensorName = forceSensorDefinition.getSensorName(); if (sensorsToIgnore.contains(sensorName)) continue; YoFrameVector intermediateSignal = intermediateForceSensorSignals.get(forceSensorDefinition); List<ProcessingYoVariable> processors = processedForceSensorSignals.get(forceSensorDefinition); String prefix = sensorType.getProcessorNamePrefix(ALPHA_FILTER); int newProcessorID = processors.size(); processorsIDs.put(sensorName, newProcessorID); String suffix = sensorType.getProcessorNameSuffix(sensorName, newProcessorID); AlphaFilteredYoFrameVector filter = AlphaFilteredYoFrameVector.createAlphaFilteredYoFrameVector(prefix, suffix, registry, alphaFilter, intermediateSignal); processors.add(filter); if (!forVizOnly) intermediateForceSensorSignals.put(forceSensorDefinition, filter); } return processorsIDs; }
/** * The estimated state of the whole robot is packed and sent to the GUI using the DRCJointConfigurationData packet. * Hackish shit going on around here: the lidar and finger joints are not packed in the packet. * @param estimatorModel * @param forceSensorDataHolderForEstimator */ public JointConfigurationGatherer(FullHumanoidRobotModel estimatorModel, ForceSensorDataHolderReadOnly forceSensorDataHolderForEstimator) { this.rootJoint = estimatorModel.getRootJoint(); FullRobotModelUtils.getAllJointsExcludingHands(joints, estimatorModel); forceSensorDefinitions = forceSensorDataHolderForEstimator.getForceSensorDefinitions().toArray(new ForceSensorDefinition[forceSensorDataHolderForEstimator.getForceSensorDefinitions().size()]); for (ForceSensorDefinition definition : forceSensorDefinitions) { String sensorName = definition.getSensorName(); forceSensorNameList.add(sensorName); ForceSensorDataReadOnly forceSensorData = forceSensorDataHolderForEstimator.get(definition); forceSensorDataList.add(forceSensorData); } }
public WrenchSensorValidityChecker(ForceSensorDefinition forceSensorDefinition, YoFrameVector forceMeasurement, YoFrameVector torqueMeasurement, YoVariableRegistry parentRegistry) { String wrenchSensorName = forceSensorDefinition.getSensorName(); registry = new YoVariableRegistry(wrenchSensorName + "WrenchSensorValidityChecker"); parentRegistry.addChild(registry); verifyYoVariableNames(wrenchSensorName, forceMeasurement, torqueMeasurement); forceChecker = new YoFrameTupleValidityChecker(forceMeasurement, registry); torqueChecker = new YoFrameTupleValidityChecker(torqueMeasurement, registry); }
private void setupForceSensorMassCompensators(FullRobotModel estimatorModel, SideDependentList<String> wristForceSensorNames) { ForceSensorDefinition[] forceSensorDefinitions = estimatorModel.getForceSensorDefinitions(); for (int i = 0; i < forceSensorDefinitions.length; i++) { ForceSensorDefinition sensorDef = forceSensorDefinitions[i]; String forceSensorName = sensorDef.getSensorName(); for (RobotSide robotSide : RobotSide.values) { if (forceSensorName.equals(wristForceSensorNames.get(robotSide))) { ForceSensorDistalMassCompensator massComp = new ForceSensorDistalMassCompensator(sensorDef, WORKER_SLEEP_TIME_MILLIS, registry); wristForceSensorDistalMassCompensators.put(robotSide, massComp); ReferenceFrame sensorFrame = sensorDef.getSensorFrame(); wristForceSensorFrames.put(robotSide, sensorFrame); } } } }
public YoForceTorqueSensorHandle(ForceTorqueSensorHandle handle, ForceSensorDefinition forceSensorDefinition, YoVariableRegistry parentRegistry) { this.handle = handle; this.forceSensorDefinition = forceSensorDefinition; String name = forceSensorDefinition.getSensorName(); YoVariableRegistry registry = new YoVariableRegistry(name); this.tx = new YoDouble(name + "_tx", registry); this.ty = new YoDouble(name + "_ty", registry); this.tz = new YoDouble(name + "_tz", registry); this.fx = new YoDouble(name + "_fx", registry); this.fy = new YoDouble(name + "_fy", registry); this.fz = new YoDouble(name + "_fz", registry); }
if (definition.getSensorName().equals(sensorInformation.getFeetForceSensorNames().get(robotSide)))
for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) if (forceSensorDefinition.getSensorName().equals(forceSensorName))
for (ForceSensorDefinition forceSensorDefinition : forceSensorDefinitions) if (forceSensorDefinition.getSensorName().equals(forceSensorName))
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); }