@Override public double getEstimatedJointTorqueOffset(OneDoFJoint joint) { DiagnosticsWhenHangingHelper helper = helpers.get(joint); return helper == null ? Double.NaN : helper.getTorqueOffset(); }
@Override public double getEstimatedJointTorqueOffset(OneDoFJoint joint) { DiagnosticsWhenHangingHelper helper = helpers.get(joint); return helper == null ? 0.0 : helper.getTorqueOffset(); }
@Override public double getEstimatedJointTorqueOffset(OneDoFJointBasics joint) { DiagnosticsWhenHangingHelper helper = helpers.get(joint); return helper == null ? Double.NaN : helper.getTorqueOffset(); }
@Override public double getEstimatedJointTorqueOffset(OneDoFJointBasics joint) { DiagnosticsWhenHangingHelper helper = helpers.get(joint); return helper == null ? 0.0 : helper.getTorqueOffset(); }
public double[] getAnkleTorqueOffsets(RobotSide robotSide) { OneDoFJointBasics anklePitch = fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_PITCH); OneDoFJointBasics ankleRoll = fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_ROLL); double anklePitchTorqueOffset = helpers.get(anklePitch).getTorqueOffset(); double anklePitchTorqueOffsetSign = torqueOffsetSigns.get(anklePitch); double ankleRollTorqueOffset = helpers.get(ankleRoll).getTorqueOffset(); double ankleRollTorqueOffsetSign = torqueOffsetSigns.get(ankleRoll); return new double[] {anklePitchTorqueOffset * anklePitchTorqueOffsetSign, ankleRollTorqueOffset * ankleRollTorqueOffsetSign}; }
public double[] getWaistTorqueOffsets() { OneDoFJoint waistPitch = fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH); OneDoFJoint waistRoll = fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL); double waistPitchTorqueOffset = helpers.get(waistPitch).getTorqueOffset(); double waistPitchTorqueOffsetSign = torqueOffsetSigns.get(waistPitch); double waistRollTorqueOffset = helpers.get(waistRoll).getTorqueOffset(); double waistRollTorqueOffsetSign = torqueOffsetSigns.get(waistRoll); return new double[] { waistPitchTorqueOffset * waistPitchTorqueOffsetSign, waistRollTorqueOffset * waistRollTorqueOffsetSign }; }
public double[] getWaistTorqueOffsets() { OneDoFJointBasics waistPitch = fullRobotModel.getSpineJoint(SpineJointName.SPINE_PITCH); OneDoFJointBasics waistRoll = fullRobotModel.getSpineJoint(SpineJointName.SPINE_ROLL); double waistPitchTorqueOffset = helpers.get(waistPitch).getTorqueOffset(); double waistPitchTorqueOffsetSign = torqueOffsetSigns.get(waistPitch); double waistRollTorqueOffset = helpers.get(waistRoll).getTorqueOffset(); double waistRollTorqueOffsetSign = torqueOffsetSigns.get(waistRoll); return new double[] {waistPitchTorqueOffset * waistPitchTorqueOffsetSign, waistRollTorqueOffset * waistRollTorqueOffsetSign}; }
public double[] getAnkleTorqueOffsets(RobotSide robotSide) { OneDoFJoint anklePitch = fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_PITCH); OneDoFJoint ankleRoll = fullRobotModel.getLegJoint(robotSide, LegJointName.ANKLE_ROLL); double anklePitchTorqueOffset = helpers.get(anklePitch).getTorqueOffset(); double anklePitchTorqueOffsetSign = torqueOffsetSigns.get(anklePitch); double ankleRollTorqueOffset = helpers.get(ankleRoll).getTorqueOffset(); double ankleRollTorqueOffsetSign = torqueOffsetSigns.get(ankleRoll); return new double[] { anklePitchTorqueOffset * anklePitchTorqueOffsetSign, ankleRollTorqueOffset * ankleRollTorqueOffsetSign }; }
public void transferTorqueOffsetsToOutputWriter() { if (jointTorqueOffsetProcessor == null) return; for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoints.get(i)); if (diagnosticsWhenHangingHelper != null) { double torqueOffset = diagnosticsWhenHangingHelper.getTorqueOffset(); jointTorqueOffsetProcessor.subtractTorqueOffset(oneDoFJoints.get(i), torqueOffset); diagnosticsWhenHangingHelper.setTorqueOffset(0.0); } } }
public void transferTorqueOffsetsToOutputWriter() { if (jointTorqueOffsetProcessor == null) return; for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoints.get(i)); if (diagnosticsWhenHangingHelper != null) { double torqueOffset = diagnosticsWhenHangingHelper.getTorqueOffset(); jointTorqueOffsetProcessor.subtractTorqueOffset(oneDoFJoints.get(i), torqueOffset); diagnosticsWhenHangingHelper.setTorqueOffset(0.0); } } }
public void transferTorqueOffsetsToOutputWriter() { if (jointTorqueOffsetProcessor == null) return; for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper helper = helpers.get(oneDoFJoints.get(i)); if (helper != null) { double torqueOffset = helper.getTorqueOffset(); jointTorqueOffsetProcessor.subtractTorqueOffset(oneDoFJoints.get(i), torqueOffset); helper.setTorqueOffset(0.0); } } }
public void transferTorqueOffsetsToOutputWriter() { if (jointTorqueOffsetProcessor == null) return; for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper helper = helpers.get(oneDoFJoints.get(i)); if (helper != null) { double torqueOffset = helper.getTorqueOffset(); jointTorqueOffsetProcessor.subtractTorqueOffset(oneDoFJoints.get(i), torqueOffset); helper.setTorqueOffset(0.0); } } }
@Override public void processDataAtControllerRate() { logDataProcessorHelper.update(); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { diagnosticsWhenHangingHelper.update(); if (tauOutput.get(oneDoFJoint) != null) tauOutput.get(oneDoFJoint).set(diagnosticsWhenHangingHelper.getEstimatedTorque() + diagnosticsWhenHangingHelper.getTorqueOffset()); } } }
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); } } }
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); } } }
private void updatePDController(OneDoFJoint oneDoFJoint, double timeInCurrentState) { PDController pdController = pdControllers.get(oneDoFJoint); double desiredPosition = desiredPositions.get(oneDoFJoint).getDoubleValue(); double desiredVelocity = 0.0; double tau = pdController.compute(oneDoFJoint.getQ(), desiredPosition, oneDoFJoint.getQd(), desiredVelocity); DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { tau = diagnosticsWhenHangingHelper.getTorqueToApply(tau, estimateTorqueOffset.getBooleanValue(), maximumTorqueOffset.getDoubleValue()); if (hasReachedMaximumTorqueOffset.getBooleanValue() && Math.abs(diagnosticsWhenHangingHelper.getTorqueOffset()) == maximumTorqueOffset.getDoubleValue()) { PrintTools.warn(this, "Reached maximum torque for at least one joint."); hasReachedMaximumTorqueOffset.set(true); } } double ditherTorque = ditherAmplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * ditherFrequency.getDoubleValue() * timeInCurrentState); oneDoFJoint.setTau(tau + ditherTorque); }
private void updatePDController(OneDoFJointBasics oneDoFJoint, double timeInCurrentState) { PDController pdController = pdControllers.get(oneDoFJoint); double desiredPosition = desiredPositions.get(oneDoFJoint).getDoubleValue(); double desiredVelocity = 0.0; double tau = pdController.compute(oneDoFJoint.getQ(), desiredPosition, oneDoFJoint.getQd(), desiredVelocity); DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { tau = diagnosticsWhenHangingHelper.getTorqueToApply(tau, estimateTorqueOffset.getBooleanValue(), maximumTorqueOffset.getDoubleValue()); if (hasReachedMaximumTorqueOffset.getBooleanValue() && Math.abs(diagnosticsWhenHangingHelper.getTorqueOffset()) == maximumTorqueOffset.getDoubleValue()) { PrintTools.warn(this, "Reached maximum torque for at least one joint."); hasReachedMaximumTorqueOffset.set(true); } } double ditherTorque = ditherAmplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * ditherFrequency.getDoubleValue() * timeInCurrentState); oneDoFJoint.setTau(tau + ditherTorque); }