@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 addOffsetTorquesToAppliedTorques() { for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoints.get(i)); if (diagnosticsWhenHangingHelper != null) diagnosticsWhenHangingHelper.addOffsetToEstimatedTorque(); } }
public double getAppliedTorque(OneDoFJoint oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) return diagnosticsWhenHangingHelper.getAppliedTorque(); return 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); } } }
@Override public double getEstimatedJointTorqueOffset(OneDoFJoint joint) { DiagnosticsWhenHangingHelper helper = helpers.get(joint); return helper == null ? Double.NaN : helper.getTorqueOffset(); }
private void makeLegJointHelper(RobotSide robotSide, double torqueOffsetSign, boolean preserveY, LegJointName legJointName) { OneDoFJointBasics legJoint = fullRobotModel.getLegJoint(robotSide, legJointName); helpers.put(legJoint, new DiagnosticsWhenHangingHelper(legJoint, preserveY, registry)); torqueOffsetSigns.put(legJoint, torqueOffsetSign); }
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); }
public void updateDiagnosticsWhenHangingHelpers() { for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoints.get(i)); if (diagnosticsWhenHangingHelper != null) diagnosticsWhenHangingHelper.update(); } }
public double getEstimatedTorque(OneDoFJoint oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) return diagnosticsWhenHangingHelper.getEstimatedTorque(); return 0.0; }
public YoDouble getTorqueOffsetVariable(OneDoFJointBasics oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { return diagnosticsWhenHangingHelper.getTorqueOffsetVariable(); } return null; }
public DoubleYoVariable getEstimatedTorqueYoVariable(OneDoFJoint oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) return diagnosticsWhenHangingHelper.getEstimatedTorqueYoVariable(); return null; }
public YoDouble getAppliedTorqueYoVariable(OneDoFJointBasics oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) return diagnosticsWhenHangingHelper.getAppliedTorqueYoVariable(); return null; }
private void updatePDController(OneDoFJointBasics oneDoFJoint, double timeInCurrentState) { PDController pdController = pdControllers.get(oneDoFJoint); YoMinimumJerkTrajectory transitionSpline = transitionSplines.get(oneDoFJoint); double desiredPosition = transitionSpline.getPosition(); double desiredVelocity = transitionSpline.getVelocity(); // Setting the desired positions via SCS ui. if (manualMode.getBooleanValue()) { desiredPosition = desiredPositions.get(oneDoFJoint).getDoubleValue(); desiredVelocity = 0.0; } else { desiredPositions.get(oneDoFJoint).set(desiredPosition); } desiredVelocities.get(oneDoFJoint).set(desiredVelocity); double tau = pdController.compute(oneDoFJoint.getQ(), desiredPosition, oneDoFJoint.getQd(), desiredVelocity); tau = tau * diagnosticsPDMasterGain.getDoubleValue(); DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { tau = diagnosticsWhenHangingHelper.getTorqueToApply(tau, adaptTorqueOffset.getBooleanValue(), maximumTorqueOffset.getDoubleValue()); } double ditherTorque = ditherAmplitude.getDoubleValue() * Math.sin(2.0 * Math.PI * ditherFrequency.getDoubleValue() * timeInCurrentState); oneDoFJoint.setTau(tau + ditherTorque); }
@Override public double getEstimatedJointTorqueOffset(OneDoFJoint joint) { DiagnosticsWhenHangingHelper helper = helpers.get(joint); return helper == null ? 0.0 : helper.getTorqueOffset(); }
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); } } }
private void makeLegJointHelper(RobotSide robotSide, boolean preserveY, LegJointName legJointName) { OneDoFJointBasics legJoint = fullRobotModel.getLegJoint(robotSide, legJointName); helpers.put(legJoint, new DiagnosticsWhenHangingHelper(legJoint, preserveY, registry)); }
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); }
public void updateDiagnosticsWhenHangingHelpers() { for (int i = 0; i < oneDoFJoints.size(); i++) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoints.get(i)); if (diagnosticsWhenHangingHelper != null) diagnosticsWhenHangingHelper.update(); } }
public double getEstimatedTorque(OneDoFJointBasics oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) return diagnosticsWhenHangingHelper.getEstimatedTorque(); return 0.0; }
public DoubleYoVariable getTorqueOffsetVariable(OneDoFJoint oneDoFJoint) { DiagnosticsWhenHangingHelper diagnosticsWhenHangingHelper = helpers.get(oneDoFJoint); if (diagnosticsWhenHangingHelper != null) { return diagnosticsWhenHangingHelper.getTorqueOffsetVariable(); } return null; }