public WrenchAndContactSensorFusedFootSwitch(String namePrefix, ForceSensorDataReadOnly forceSensorData, ContactSensor contactSensor, double footSwitchCoPThresholdFraction, double robotTotalWeight, ContactablePlaneBody contactablePlaneBody, YoGraphicsListRegistry yoGraphicsListRegistry, double contactThresholdForce, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); if (!forceSensorData.getMeasurementLink().equals(contactSensor.getRigidBody())) { throw new RuntimeException("The force sensor and the contact sensor are not on the same link."); } this.wrenchBasedFootSwitch = new WrenchBasedFootSwitch(namePrefix + "WrenchBasedFootSwitch", forceSensorData, footSwitchCoPThresholdFraction, robotTotalWeight, contactablePlaneBody, yoGraphicsListRegistry, contactThresholdForce, registry); this.contactSensor = contactSensor; this.inContact = new BooleanYoVariable(namePrefix + "InContact", registry); parentRegistry.addChild(registry); }