public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }
public PassiveJointController(OneDegreeOfFreedomJoint simulatedJoint, YoPDGains gains) { registry = new YoVariableRegistry(simulatedJoint.getName() + name); jointController = new PDController(gains.getYoKp(), gains.getYoKd(), simulatedJoint.getName() + "PassiveController", registry); this.simulatedJoint = simulatedJoint; jointController.setProportionalGain(36000.0); jointController.setDerivativeGain(1000.0); }
public PDController(YoPDGains pdGains, String suffix, YoVariableRegistry registry) { this.proportionalGain = pdGains.getYoKp(); this.derivativeGain = pdGains.getYoKd(); this.positionDeadband = pdGains.getPositionDeadband(); positionError = new DoubleYoVariable("positionError_" + suffix, registry); positionError.set(0.0); rateError = new DoubleYoVariable("rateError_" + suffix, registry); rateError.set(0.0); actionP = new DoubleYoVariable("action_P_" + suffix, registry); actionP.set(0.0); actionD = new DoubleYoVariable("action_D_" + suffix, registry); actionD.set(0.0); }
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); // TODO: Fix low level stuff so that we are truly controlling pelvis height and not CoM height. 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(); // TODO Need to extract the maximum velocity parameter. coMHeightTimeDerivativesSmoother = new CoMHeightTimeDerivativesSmoother(null, maxCoMHeightAcceleration, maxCoMHeightJerk, controlDT, registry); this.centerOfMassHeightController = new PDController(kpCoMHeight, kdCoMHeight, "comHeight", registry); parentRegistry.addChild(registry); }