@Override public HighLevelBehavior createHighLevelBehavior(HighLevelControlManagerFactory variousWalkingManagers, HighLevelHumanoidControllerToolbox momentumBasedController) { controller = new DiagnosticsWhenHangingController(humanoidJointPoseList, useArms, robotIsHanging, momentumBasedController, this.torqueOffsetPrinter); controller.addUpdatables(updatables); if (jointTorqueOffsetProcessor != null) controller.attachjointTorqueOffsetProcessor(jointTorqueOffsetProcessor); return controller; }
@Override public void doAction() { doControl(); }
@Override public void doControl() callUpdatables(); updateDiagnosticsWhenHangingHelpers(); updatePDControllers(); if (updateFootForceSensorOffsets.getBooleanValue()) updateFootSensorRawMeasurement(); stateMachine.checkTransitionConditions(); transferTorqueOffsetsToOutputWriter(); printFootSensorsOffset();
setDesiredPositionsFromPoseList(finalPositions); copyFinalDesiredPositionsToInitialDesired(); setDefaultPDControllerGains(); createTransitionSplines(); createHelpers(robotIsHanging); setTransitionSplines();
@Override public void processData() FullRobotModel fullRobotModel = controller.getFullRobotModel(); fullRobotModel.updateFrames(); controller.updateDiagnosticsWhenHangingHelpers(); controller.addOffsetTorquesToAppliedTorques(); Double torqueScoreValue = torqueScoreValues.get(oneDoFJoint); double appliedTorque = controller.getAppliedTorque(oneDoFJoint); double estimatedTorque = controller.getEstimatedTorque(oneDoFJoint);
public void attachJointTorqueOffsetProcessor(JointTorqueOffsetProcessor jointTorqueOffsetProcessor) { if (controller != null) controller.attachjointTorqueOffsetProcessor(jointTorqueOffsetProcessor); else this.jointTorqueOffsetProcessor = jointTorqueOffsetProcessor; }