public CenterOfMassHeightManager(HighLevelHumanoidControllerToolbox momentumBasedController, WalkingControllerParameters walkingControllerParameters,
YoVariableRegistry parentRegistry)
{
CommonHumanoidReferenceFrames referenceFrames = momentumBasedController.getReferenceFrames();
centerOfMassFrame = referenceFrames.getCenterOfMassFrame();
centerOfMassJacobian = momentumBasedController.getCenterOfMassJacobian();
pelvisFrame = referenceFrames.getPelvisFrame();
gravity = momentumBasedController.getGravityZ();
pelvis = momentumBasedController.getFullRobotModel().getPelvis();
twistCalculator = momentumBasedController.getTwistCalculator();
centerOfMassTrajectoryGenerator = createTrajectoryGenerator(momentumBasedController, walkingControllerParameters, referenceFrames);
controlPelvisHeightInsteadOfCoMHeight.set(true);
YoPDGains comHeightControlGains = walkingControllerParameters.createCoMHeightControlGains(registry);
DoubleYoVariable kpCoMHeight = comHeightControlGains.getYoKp();
DoubleYoVariable kdCoMHeight = comHeightControlGains.getYoKd();
DoubleYoVariable maxCoMHeightAcceleration = comHeightControlGains.getYoMaximumFeedback();
DoubleYoVariable maxCoMHeightJerk = comHeightControlGains.getYoMaximumFeedbackRate();
double controlDT = momentumBasedController.getControlDT();
coMHeightTimeDerivativesSmoother = new CoMHeightTimeDerivativesSmoother(null, maxCoMHeightAcceleration, maxCoMHeightJerk, controlDT, registry);
this.centerOfMassHeightController = new PDController(kpCoMHeight, kdCoMHeight, "comHeight", registry);
parentRegistry.addChild(registry);
}