public void printOffsetsForCoeffsForValkyrie() { java.text.NumberFormat doubleFormat = new java.text.DecimalFormat(" 0.00;-0.00"); System.out.println(); ArrayList<OneDoFJointBasics> oneDoFJoints = diagnosticsWhenHangingController.getOneDoFJoints(); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = diagnosticsWhenHangingController.getDiagnosticsWhenHangingHelper(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { double torqueOffset = diagnosticsWhenHangingHelper.getTorqueOffset(); double torqueOffsetSign = diagnosticsWhenHangingController.getTorqueOffsetSign(oneDoFJoint); double signedTorqueOffset = torqueOffset * torqueOffsetSign; String offsetString = doubleFormat.format(signedTorqueOffset); System.out.println(oneDoFJoint.getName() + " torque offset = " + offsetString); } } }
@Override public HighLevelControllerState getOrCreateControllerState(HighLevelControllerFactoryHelper controllerFactoryHelper) { controller = new DiagnosticsWhenHangingControllerState(humanoidJointPoseList, useArms, robotIsHanging, controllerFactoryHelper.getHighLevelHumanoidControllerToolbox(), controllerFactoryHelper.getHighLevelControllerParameters(), this.torqueOffsetPrinter); controller.addUpdatables(updatables); if (jointTorqueOffsetProcessor != null) controller.attachjointTorqueOffsetProcessor(jointTorqueOffsetProcessor); return controller; }
@Override public void doControl() callUpdatables(); updateDiagnosticsWhenHangingHelpers(); updatePDControllers(); if (updateFootForceSensorOffsets.getBooleanValue()) updateFootSensorRawMeasurement(); stateMachine.doTransitions(); transferTorqueOffsetsToOutputWriter(); printFootSensorsOffset(); jointDesiredOutput.setDesiredTorque(joint.getTau()); lowLevelOneDoFJointDesiredDataHolder.completeWith(getStateSpecificJointSettings());
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; }
public void printOffsetsForCoeffsForSteppr() { java.text.NumberFormat doubleFormat = new java.text.DecimalFormat(" 0.00;-0.00"); System.out.println(); ArrayList<OneDoFJoint> oneDoFJoints = diagnosticsWhenHangingController.getOneDoFJoints(); for (OneDoFJoint oneDoFJoint : oneDoFJoints) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = diagnosticsWhenHangingController.getDiagnosticsWhenHangingHelper(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { double torqueOffset = diagnosticsWhenHangingHelper.getTorqueOffset(); double torqueOffsetSign = diagnosticsWhenHangingController.getTorqueOffsetSign(oneDoFJoint); double signedTorqueOffset = torqueOffset * torqueOffsetSign; String offsetString = doubleFormat.format(signedTorqueOffset); System.out.println(oneDoFJoint.getName() + " torque offset = " + offsetString); } } }