private void updatePDController(OneDoFJoint 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); }
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); }
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); }