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