@Override public double getKp() { return limitedKp.getDoubleValue(); }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, referenceFrame); return ret; }
@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); } }
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)); } } } }
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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testUpdateWithMaxRateBeingNegative() { try { RateLimitedYoVariable rateLimitedYoVariableWithNegativeMaxRate = new RateLimitedYoVariable("rateLimitedYoVariableWithNegativeMaxRate", registry, -5.0, 1.0); rateLimitedYoVariableWithNegativeMaxRate.update(5.0); } catch (RuntimeException e) { assertTrue(e.getMessage() == "The maxRate parameter in the RateLimitedYoVariable cannot be negative."); } } }
public void resetStictionCompensation() { this.stictionCompensationLimit.set(0.0); this.stictionCompensation.set(0.0); }
public void setMaxRate(double maxRate) { x.setMaxRate(maxRate); y.setMaxRate(maxRate); }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, double dt, ReferenceFrame referenceFrame) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, referenceFrame); return ret; }
if (Math.abs(currentPosition1 - rateLimitedYoVariable1.getDoubleValue()) > maxRate1) currentPosition1 = rateLimitedYoVariable1.getDoubleValue() + signOfSlope * maxRate1; rateLimitedYoVariable1.update(currentPosition1); assertEquals(rateLimitedYoVariable1.getDoubleValue(), currentPosition1, 1E-15); if (Math.abs(currentPosition2 - rateLimitedYoVariable2.getDoubleValue()) > maxRate2.getDoubleValue()) currentPosition2 = rateLimitedYoVariable2.getDoubleValue() + signOfSlope * maxRate2.getDoubleValue(); rateLimitedYoVariable2.update(currentPosition2); assertEquals(rateLimitedYoVariable2.getDoubleValue(), currentPosition2, 1E-15); if (Math.abs(currentPosition3 - rateLimitedYoVariable3.getDoubleValue()) > maxRate3) currentPosition3 = rateLimitedYoVariable3.getDoubleValue() + signOfSlope * maxRate3; rateLimitedYoVariable3.update(currentPosition3); assertEquals(rateLimitedYoVariable3.getDoubleValue(), currentPosition3, 1E-15); if (Math.abs(currentPosition4 - rateLimitedYoVariable4.getDoubleValue()) > maxRate4.getDoubleValue()) currentPosition4 = rateLimitedYoVariable4.getDoubleValue() + signOfSlope * maxRate4.getDoubleValue(); rateLimitedYoVariable4.update(currentPosition4); assertEquals(rateLimitedYoVariable4.getDoubleValue(), currentPosition4, 1E-15);
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 static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, ReferenceFrame referenceFrame) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); return ret; }
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 double getKi() { return limitedKi.getDoubleValue(); }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, ReferenceFrame referenceFrame) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, referenceFrame); return ret; }