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; }
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; }
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; }
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; }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable 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 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 YoLimitedPDGains(String suffix, double controlDT, YoVariableRegistry registry) { super(suffix, registry); maxKpRate = new DoubleYoVariable("maxKpRate" + suffix, registry); maxKdRate = new DoubleYoVariable("maxKdRate" + suffix, registry); limitedKp = new RateLimitedYoVariable("limitedKp" + suffix, registry, maxKpRate, kp, controlDT); limitedKd = new RateLimitedYoVariable("limitedKd" + suffix, registry, maxKdRate, kd, controlDT); maxKpRate.set(Double.POSITIVE_INFINITY); maxKdRate.set(Double.POSITIVE_INFINITY); }
public YoLimitedPIDGains(String suffix, double controlDT, YoVariableRegistry registry) { super(suffix, registry); maxKpRate = new DoubleYoVariable("maxKpRate" + suffix, registry); maxKdRate = new DoubleYoVariable("maxKdRate" + suffix, registry); maxKiRate = new DoubleYoVariable("maxKiRate" + suffix, registry); limitedKi = new RateLimitedYoVariable("limitedKi" + suffix, registry, maxKiRate, ki, controlDT); limitedKp = new RateLimitedYoVariable("limitedKp" + suffix, registry, maxKpRate, kp, controlDT); limitedKd = new RateLimitedYoVariable("limitedKd" + suffix, registry, maxKdRate, kd, controlDT); maxKpRate.set(Double.POSITIVE_INFINITY); maxKdRate.set(Double.POSITIVE_INFINITY); maxKiRate.set(Double.POSITIVE_INFINITY); }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameVector unfilteredVector) { // alpha is a double RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoZ(), dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector createRateLimitedYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, double dt, YoFrameVector unfilteredVector) { // alpha is a YoVariable RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoVariable z = new RateLimitedYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoZ(), dt); RateLimitedYoFrameVector ret = new RateLimitedYoFrameVector(x, y, z, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maxRate, double dt, YoFrameVector2D unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable maxRate, double dt, YoFrameVector2d unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameVector2d unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
public static RateLimitedYoFrameVector2d createRateLimitedYoFrameVector2d(String namePrefix, String nameSuffix, YoVariableRegistry registry, double maxRate, double dt, YoFrameVector2D unfilteredVector) { RateLimitedYoVariable x = new RateLimitedYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoX(), dt); RateLimitedYoVariable y = new RateLimitedYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, maxRate, unfilteredVector.getYoY(), dt); RateLimitedYoFrameVector2d ret = new RateLimitedYoFrameVector2d(x, y, unfilteredVector.getReferenceFrame()); return ret; }
public YoLimitedPDGains(String suffix, double controlDT, YoVariableRegistry registry) { super(suffix, registry); maxKpRate = new YoDouble("maxKpRate" + suffix, registry); maxKdRate = new YoDouble("maxKdRate" + suffix, registry); limitedKp = new RateLimitedYoVariable("limitedKp" + suffix, registry, maxKpRate, super.getYoKp(), controlDT); limitedKd = new RateLimitedYoVariable("limitedKd" + suffix, registry, maxKdRate, super.getYoKd(), controlDT); maxKpRate.set(Double.POSITIVE_INFINITY); maxKdRate.set(Double.POSITIVE_INFINITY); }
@Before public void setUp() { maxRate2 = new YoDouble("maxRate2", registry); maxRate4 = new YoDouble("maxRate4", registry); position3 = new YoDouble("position3", registry); position4 = new YoDouble("position4", registry); maxRate1 = 10.0; maxRate2.set(9.0); maxRate3 = 11.0; maxRate4.set(12.0); dt1 = 1.0; dt2 = 1.0; dt3 = 1.0; dt4 = 1.0; position3.set(0.5); position4.set(0.75); rateLimitedYoVariable1 = new RateLimitedYoVariable("rateLimitedYoVariable1", registry, maxRate1, dt1); rateLimitedYoVariable2 = new RateLimitedYoVariable("rateLimitedYoVariable2", registry, maxRate2, dt2); rateLimitedYoVariable3 = new RateLimitedYoVariable("rateLimitedYoVariable3", registry, maxRate3, position3, dt3); rateLimitedYoVariable4 = new RateLimitedYoVariable("rateLimitedYoVariable4", registry, maxRate4, position4, dt4); }
public YoLimitedPIDGains(String suffix, double controlDT, YoVariableRegistry registry) { super(suffix, registry); maxKpRate = new YoDouble("maxKpRate" + suffix, registry); maxKdRate = new YoDouble("maxKdRate" + suffix, registry); maxKiRate = new YoDouble("maxKiRate" + suffix, registry); limitedKi = new RateLimitedYoVariable("limitedKi" + suffix, registry, maxKiRate, super.getYoKi(), controlDT); limitedKp = new RateLimitedYoVariable("limitedKp" + suffix, registry, maxKpRate, super.getYoKp(), controlDT); limitedKd = new RateLimitedYoVariable("limitedKd" + suffix, registry, maxKdRate, super.getYoKd(), controlDT); maxKpRate.set(Double.POSITIVE_INFINITY); maxKdRate.set(Double.POSITIVE_INFINITY); maxKiRate.set(Double.POSITIVE_INFINITY); }
@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 StictionCompensator(String prefix, StictionModel stictionModel, double controlDT, YoVariableRegistry parentRegistry) { this.stictionModel = stictionModel; this.controlDt = controlDT; YoVariableRegistry registry = new YoVariableRegistry(prefix + getClass().getSimpleName()); movingVelocityThreshold = new DoubleParameter(prefix + "_MovingVelocityThreshold", registry, 1e-2); movingAccelerationThreshold = new DoubleParameter(prefix + "_MovingAccelerationThreshold", registry, 1e-1); acceleratingThreshold = new DoubleParameter(prefix + "_AcceleratingThreshold", registry, 1); desiredTorqueStictionLimitFactor = new DoubleParameter(prefix + "_DesiredTorqueStictionLimitFactor", registry, 2.0); stictionCompensationLimit = new YoDouble(prefix + "_StictionCompensationLimit", registry); stictionCompensationRate = new DoubleParameter(prefix + "_StictionCompensationRate", registry, 10.0); stictionCompensation = new RateLimitedYoVariable(prefix + "_StictionCompensation", registry, stictionCompensationRate, controlDT); stictionActionMode = new YoEnum<>(prefix + "_StictionActionMode", registry, StictionActionMode.class); windowSize = new YoInteger(prefix + "_StictionActionModeWindowSize", registry); stictionActionModeIndex = new GlitchFilteredYoInteger(prefix + "_StictionActionModeIndex", windowSize, registry); minTimeInMode = new DoubleParameter(prefix + "_MinTimeInMode", registry, 5e-2); timeInCurrentMode = new YoDouble(prefix + "_TimeInCurrentMode", registry); stictionActionModeIndex.set(StictionActionMode.Stopped.index()); stictionActionMode.set(StictionActionMode.Stopped); parentRegistry.addChild(registry); }
public OneDoFJointFeedbackController(OneDoFJoint joint, double dt, YoVariableRegistry parentRegistry) { String jointName = joint.getName(); YoVariableRegistry registry = new YoVariableRegistry(jointName + "PDController"); this.joint = joint; isEnabled = new BooleanYoVariable("control_enabled_" + jointName, registry); isEnabled.set(false); qCurrent = new DoubleYoVariable("q_" + jointName, registry); qDCurrent = new DoubleYoVariable("qd_" + jointName, registry); qDesired = new DoubleYoVariable("q_d_" + jointName, registry); qDDesired = new DoubleYoVariable("qd_d_" + jointName, registry); qDDFeedforward = new DoubleYoVariable("qdd_ff_" + jointName, registry); qError = new DoubleYoVariable("q_err_" + jointName, registry); qDError = new DoubleYoVariable("qd_err_" + jointName, registry); maxFeedbackAcceleration = new DoubleYoVariable("qdd_max_fb_" + jointName, registry); maxFeedbackJerk = new DoubleYoVariable("qddd_fb_max_" + jointName, registry); qDDFeedback = new DoubleYoVariable("qdd_fb_" + jointName, registry); qDDFeedbackRateLimited = new RateLimitedYoVariable("qdd_fb_rl_" + jointName, registry, maxFeedbackJerk, qDDFeedback, dt); qDDDesired = new DoubleYoVariable("qdd_d_" + jointName, registry); qDDAchieved = new DoubleYoVariable("qdd_achieved_" + jointName, registry); kp = new DoubleYoVariable("kp_" + jointName, registry); kd = new DoubleYoVariable("kd_" + jointName, registry); weightForSolver = new DoubleYoVariable("weight_" + jointName, registry); weightForSolver.set(Double.POSITIVE_INFINITY); output.addJoint(joint, Double.NaN); parentRegistry.addChild(registry); }