@Override public double getKd() { return limitedKd.getDoubleValue(); }
@Override public double getKd() { return limitedKd.getDoubleValue(); }
@Override public double getKp() { return limitedKp.getDoubleValue(); }
@Override public double getKi() { return limitedKi.getDoubleValue(); }
@Override public double getKd() { return limitedKd.getDoubleValue(); }
@Override public double getKd() { return limitedKd.getDoubleValue(); }
@Override public double getKp() { return limitedKp.getDoubleValue(); }
@Override public double getKp() { return limitedKp.getDoubleValue(); }
@Override public double getKp() { return limitedKp.getDoubleValue(); }
public double getStictionCompensation() { return stictionCompensation.getDoubleValue(); }
double signOfSlope = Math.signum(dSinTheta) * 1.0; if (Math.abs(currentPosition1 - rateLimitedYoVariable1.getDoubleValue()) > maxRate1) currentPosition1 = rateLimitedYoVariable1.getDoubleValue() + signOfSlope * maxRate1; assertEquals(rateLimitedYoVariable1.getDoubleValue(), currentPosition1, 1E-15); double signOfSlope = Math.signum(dSinTheta) * 1.0; if (Math.abs(currentPosition2 - rateLimitedYoVariable2.getDoubleValue()) > maxRate2.getDoubleValue()) currentPosition2 = rateLimitedYoVariable2.getDoubleValue() + signOfSlope * maxRate2.getDoubleValue(); assertEquals(rateLimitedYoVariable2.getDoubleValue(), currentPosition2, 1E-15); if (Math.abs(currentPosition3 - rateLimitedYoVariable3.getDoubleValue()) > maxRate3) currentPosition3 = rateLimitedYoVariable3.getDoubleValue() + signOfSlope * maxRate3; assertEquals(rateLimitedYoVariable3.getDoubleValue(), currentPosition3, 1E-15); if (Math.abs(currentPosition4 - rateLimitedYoVariable4.getDoubleValue()) > maxRate4.getDoubleValue()) currentPosition4 = rateLimitedYoVariable4.getDoubleValue() + signOfSlope * maxRate4.getDoubleValue(); assertEquals(rateLimitedYoVariable4.getDoubleValue(), currentPosition4, 1E-15);
public void update(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); set(currentPosition); } final double maxRateToUse; if (maxRateVariable != null) maxRateToUse = maxRateVariable.getDoubleValue(); else maxRateToUse = maxRate; if (maxRateToUse < 0) throw new RuntimeException("The maxRate parameter in the RateLimitedYoVariable constructor cannot be negative."); double difference = currentPosition - getDoubleValue(); if (Math.abs(difference) > maxRateToUse * dt) difference = Math.signum(difference) * maxRateToUse * dt; set(getDoubleValue() + difference); } }
private void updateLowLevelData(Map<InverseDynamicsJoint, Double> jointTorquesSolution) { rootJointDesiredConfiguration.setDesiredAccelerationFromJoint(rootJoint); for (OneDoFJoint joint : controlledOneDoFJoints) { if (jointTorquesSolution.containsKey(joint)) { if (USE_LIMITED_JOINT_TORQUES) { if (firstTick) jointTorqueSolutions.get(joint).set(jointTorquesSolution.get(joint)); else jointTorqueSolutions.get(joint).update(jointTorquesSolution.get(joint)); lowLevelOneDoFJointDesiredDataHolder.setDesiredJointTorque(joint, jointTorqueSolutions.get(joint).getDoubleValue()); } else { lowLevelOneDoFJointDesiredDataHolder.setDesiredJointTorque(joint, jointTorquesSolution.get(joint)); } } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testUpdateWithCurrentPositionParameter() { for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) { double currentPosition1 = 10.0 * Math.sin(angle); rateLimitedYoVariable1.update(currentPosition1); assertEquals(rateLimitedYoVariable1.getDoubleValue(), currentPosition1, 1E-13); } for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) { double currentPosition2 = 7.0 * Math.sin(angle); rateLimitedYoVariable2.update(currentPosition2); assertEquals(rateLimitedYoVariable2.getDoubleValue(), currentPosition2, 1E-13); } for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) { double currentPosition3 = 11.0 * Math.sin(angle); rateLimitedYoVariable3.update(currentPosition3); assertEquals(rateLimitedYoVariable3.getDoubleValue(), currentPosition3, 1E-13); } for (double angle = 0.0; angle < 3 * 6.28; angle += 1.0) { double currentPosition4 = 12.0 * Math.sin(angle); rateLimitedYoVariable4.update(currentPosition4); assertEquals(rateLimitedYoVariable4.getDoubleValue(), currentPosition4, 1E-13); } }
public void update(double currentPosition) { if (!hasBeenCalled.getBooleanValue()) { hasBeenCalled.set(true); set(currentPosition); } if (maxRateVariable.getValue() < 0) throw new RuntimeException("The maxRate parameter in the RateLimitedYoVariable cannot be negative."); double difference = currentPosition - getDoubleValue(); if (Math.abs(difference) > maxRateVariable.getValue() * dt) { difference = Math.signum(difference) * maxRateVariable.getValue() * dt; this.limited.set(true); } else this.limited.set(false); set(getDoubleValue() + difference); } }
public double computeStictionCompensation() { updateActionMode(); if (stictionActionMode.getEnumValue().active()) { double torqueSign = Math.signum(desiredTorque); double stictionMagnitude = stictionModel.getStictionMagnitude(); stictionCompensationLimit.set(Math.min(torqueSign * desiredTorque * desiredTorqueStictionLimitFactor.getValue(), stictionMagnitude)); stictionCompensation.update(torqueSign * stictionCompensationLimit.getDoubleValue()); } else { stictionCompensationLimit.set(0.0); stictionCompensation.update(stictionCompensationLimit.getDoubleValue()); } return stictionCompensation.getDoubleValue(); }
@Override public void compute() { if (!isEnabled.getBooleanValue()) return; qCurrent.set(joint.getQ()); qDCurrent.set(joint.getQd()); qError.set(qDesired.getDoubleValue() - qCurrent.getDoubleValue()); qDError.set(qDDesired.getDoubleValue() - qDCurrent.getDoubleValue()); double qdd_fb = kp.getDoubleValue() * qError.getDoubleValue() + kd.getDoubleValue() * qDError.getDoubleValue(); qdd_fb = MathTools.clipToMinMax(qdd_fb, maxFeedbackAcceleration.getDoubleValue()); qDDFeedback.set(qdd_fb); qDDFeedbackRateLimited.update(); qDDDesired.set(qDDFeedforward.getDoubleValue() + qDDFeedbackRateLimited.getDoubleValue()); output.setOneDoFJointDesiredAcceleration(0, qDDDesired.getDoubleValue()); }