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