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