public ValkyrieCalibrationControllerState(HighLevelHumanoidControllerToolbox highLevelControllerToolbox, HighLevelControllerParameters highLevelControllerParameters, JointDesiredOutputListReadOnly highLevelControlOutput, ValkyrieCalibrationParameters calibrationParameters, TorqueOffsetPrinter torqueOffsetPrinter) { super(controllerState, highLevelControllerParameters, MultiBodySystemTools.filterJoints(highLevelControllerToolbox.getControlledJoints(), OneDoFJoint.class)); this.highLevelControlOutput = highLevelControlOutput; for (OneDoFJointBasics controlledJoint : controlledJoints) { String jointName = controlledJoint.getName(); YoPolynomial trajectory = new YoPolynomial(jointName + "_CalibrationTrajectory", 4, registry); YoDouble initialPosition = new YoDouble(jointName + "_CalibrationInitialPosition", registry); jointsData.add(controlledJoint, new TrajectoryData(initialPosition, trajectory)); } timeToMoveForCalibration.set(timeToMove); timeForEstimatingOffset.set(highLevelControllerParameters.getCalibrationDuration()); jointTorqueOffsetEstimatorController = new JointTorqueOffsetEstimatorController(calibrationParameters, highLevelControllerToolbox, torqueOffsetPrinter); registry.addChild(jointTorqueOffsetEstimatorController.getYoVariableRegistry()); lowLevelOneDoFJointDesiredDataHolder.registerJointsWithEmptyData(controlledJoints); StateMachineFactory<CalibrationStates, CalibrationState> factory = new StateMachineFactory<>(CalibrationStates.class); factory.setNamePrefix("calibrationState").setRegistry(registry).buildYoClock(highLevelControllerToolbox.getYoTime()); factory.addStateAndDoneTransition(CalibrationStates.ENTRY, new CalibrationEntry(), CalibrationStates.CALIBRATE); factory.addStateAndDoneTransition(CalibrationStates.CALIBRATE, new Calibration(), CalibrationStates.EXIT); factory.addState(CalibrationStates.EXIT, new CalibrationExit()); stateMachine = factory.build(CalibrationStates.ENTRY); }