public DiagnosticsWhenHangingHelper(OneDoFJoint parentJoint, boolean preserveY, boolean isSpineJoint, SideDependentList<InverseDynamicsJoint> topLegJointsIfSpine, YoVariableRegistry registry) { this.parentJoint = parentJoint; this.isSpineJoint = isSpineJoint; centerOfMassCalculator = createCenterOfMassCalculatorInJointZUpFrame(parentJoint, preserveY, isSpineJoint, topLegJointsIfSpine); belowJointCoMInZUpFrame = new YoFramePoint(parentJoint.getName() + "CoMInZUpFrame", centerOfMassCalculator.getDesiredFrame(), registry); centerOfMassPosition = new FramePoint(centerOfMassCalculator.getDesiredFrame()); yoJointAxis = new YoFrameVector(parentJoint.getName() + "JointAxis", ReferenceFrame.getWorldFrame(), registry); yoJointToCenterOfMass = new YoFrameVector(parentJoint.getName() + "JointToCoM", ReferenceFrame.getWorldFrame(), registry); yoForceVector = new YoFrameVector(parentJoint.getName() + "ForceVector", ReferenceFrame.getWorldFrame(), registry); estimatedTorque = new DoubleYoVariable("tau_est_" + parentJoint.getName(), registry); torqueOffset = new DoubleYoVariable("tau_off_" + parentJoint.getName(), registry); appliedTorque = new DoubleYoVariable("tau_app_" + parentJoint.getName(), registry); totalMass = new DoubleYoVariable("totalMass_" + parentJoint.getName(), registry); torqueCorrectionAlpha = new DoubleYoVariable("torqueCorrectionAlpha_" + parentJoint.getName(), registry); torqueCorrectionAlpha.set(0.001); }