public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { pdController.compute(currentPosition, desiredPosition, currentRate, desiredRate); return computeIntegralEffortAndAddPDEffort(deltaTime); }
private void controlJoint(StepprJoint joint, double qDesired, double qdDesired) { OneDoFJoint oneDoFJoint = joints.get(joint); PDController controller = controllers.get(joint); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); }
@Override public void doControl(long timestamp) { for(int i = 0; i < controllers.size(); i++) { OneDoFJoint joint = joints.get(i); PDController controller = controllers.get(i); double tauFF = tauFFs.get(i).getDoubleValue(); double q_d = desiredPositions.get(i).getDoubleValue(); double qd_d = desiredVelocities.get(i).getDoubleValue(); double tau = controller.compute(joint.getQ(), q_d, joint.getQd(), qd_d) + tauFF; joint.setTau(tau); joint.setKd(damping.get(i).getDoubleValue()); } }
@Override public void doControl(long timestamp) { for(int i = 0; i < controllers.size(); i++) { OneDoFJoint joint = joints.get(i); PDController controller = controllers.get(i); double tauFF = tauFFs.get(i).getDoubleValue(); double q_d = desiredPositions.get(i).getDoubleValue(); double qd_d = desiredVelocities.get(i).getDoubleValue(); double tau = controller.compute(joint.getQ(), q_d, joint.getQd(), qd_d) + tauFF; joint.setTau(tau); joint.setKd(damping.get(i).getDoubleValue()); } }
@Override public void doControl() { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = 0.0; double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = 0.0; double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate); simulatedJoint.setTau(desiredTau); }
@Override public void doControl() { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = 0.0; double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = 0.0; double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate); simulatedJoint.setTau(desiredTau); }
public void doControl() { double currentPosition = simulatedJoint.getQYoVariable().getDoubleValue(); double desiredPosition = 0.0; double currentRate = simulatedJoint.getQDYoVariable().getDoubleValue(); double desiredRate = 0.0; double desiredTau = jointController.compute(currentPosition, desiredPosition, currentRate, desiredRate); simulatedJoint.setTau(desiredTau); }
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); }
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 doIdleControl() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJoint joint = controlledJoints.get(i); PDController jointPDController = jointPDControllerMap.get(joint); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(joint); jointTrajectory.compute(trajectoryTimeProvider.getValue()); DoubleYoVariable jointDesiredPosition = jointDesiredPositionMap.get(joint); DoubleYoVariable jointDesiredVelocity = jointDesiredVelocityMap.get(joint); jointDesiredPosition.set(jointTrajectory.getValue()); jointDesiredVelocity.set(0.0); double q = joint.getQ(); double qDesired = jointDesiredPosition.getDoubleValue(); double qd = joint.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired); double qdMax = qdMaxIdle.getDoubleValue(); double qddMax = qddMaxIdle.getDoubleValue(); double tauMax = tauMaxIdle.getDoubleValue(); qDesired = q + MathTools.clipToMinMax(qDesired - q, qdMax * controlDT); qdDesired = qd + MathTools.clipToMinMax(qdDesired - qd, qddMax * controlDT); tauDesired = MathTools.clipToMinMax(tauDesired, tauMax); joint.setTau(tauDesired); joint.setqDesired(qDesired); joint.setQdDesired(qdDesired); jointDesiredTauMap.get(joint).set(tauDesired); } }
private void doIdleControl() { for (int i = 0; i < controlledJoints.size(); i++) { OneDoFJointBasics state = controlledJoints.first(i); JointDesiredOutputBasics output = controlledJoints.second(i); PDController jointPDController = jointPDControllerMap.get(state); OneDoFJointQuinticTrajectoryGenerator jointTrajectory = jointTrajectories.get(state); jointTrajectory.compute(trajectoryTimeProvider.getValue()); YoDouble jointDesiredPosition = jointDesiredPositionMap.get(state); YoDouble jointDesiredVelocity = jointDesiredVelocityMap.get(state); jointDesiredPosition.set(jointTrajectory.getValue()); jointDesiredVelocity.set(0.0); double q = state.getQ(); double qDesired = jointDesiredPosition.getDoubleValue(); double qd = state.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired); double qdMax = qdMaxIdle.getDoubleValue(); double qddMax = qddMaxIdle.getDoubleValue(); double tauMax = tauMaxIdle.getDoubleValue(); qDesired = q + MathTools.clamp(qDesired - q, qdMax * controlDT); qdDesired = qd + MathTools.clamp(qdDesired - qd, qddMax * controlDT); tauDesired = MathTools.clamp(tauDesired, tauMax); output.setDesiredTorque(tauDesired); output.setDesiredPosition(qDesired); output.setDesiredVelocity(qdDesired); jointDesiredTauMap.get(state).set(tauDesired); } }
double qd = joint.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired) + desiredJointTauOffset; joint.setTau(tauDesired); joint.setqDesired(qDesired);
double qd = state.getQd(); double qdDesired = jointDesiredVelocity.getDoubleValue(); double tauDesired = jointPDController.compute(q, qDesired, qd, qdDesired) + desiredJointTauOffset; output.setDesiredTorque(tauDesired); output.setDesiredPosition(qDesired);
controller.setProportionalGain(kp); controller.setDerivativeGain(kd); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); oneDoFJoint.setKd(damping);
controller.setProportionalGain(kp); controller.setDerivativeGain(kd); double tau = controller.compute(oneDoFJoint.getQ(), qDesired, oneDoFJoint.getQd(), qdDesired); oneDoFJoint.setTau(tau); oneDoFJoint.setKd(damping);
@Override public void update() { joint.setQ(handle.getPosition()); joint.setQd(handle.getVelocity()); bl_qd.update(); joint.setTau(handle.getEffort()); q.set(joint.getQ()); qd.set(joint.getQd()); tau.set(joint.getTau()); double pdOutput = pdController.compute(q.getDoubleValue(), q_d.getDoubleValue(), qd.getDoubleValue(), qd_d.getDoubleValue()); jointCommand_pd.set(pdOutput); tau_d.set(valkyrieRosControlSliderBoard.masterScaleFactor.getDoubleValue() * (jointCommand_pd.getDoubleValue() + jointCommand_function.getDoubleValue()) + tau_offset .getDoubleValue()); handle.setDesiredEffort(tau_d.getDoubleValue()); } }
double zddFeedForward = comHeightDataAfterSmoothing.getComHeightAcceleration(); double zddDesired = centerOfMassHeightController.compute(zCurrent, zDesired, zdCurrent, zdDesired) + zddFeedForward;