public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable deadzoneSize, ReferenceFrame referenceFrame) { DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, referenceFrame); return ret; }
public static void main(String[] args) { YoVariableRegistry registry = new YoVariableRegistry("test"); DoubleYoVariable deadzoneSize = new DoubleYoVariable("deadzoneSize", registry); DoubleYoVariable input = new DoubleYoVariable("input", registry); deadzoneSize.set(2.0); DeadzoneYoVariable testDeadzone = new DeadzoneYoVariable("testDeadZone", input,deadzoneSize,registry); for(int i = -50; i < 51; i++) { input.set((double)i); testDeadzone.update(); System.out.println("//////////////////////////"); System.out.println("uncorrected = " + (double)i); System.out.println("corrected = " + testDeadzone.getDoubleValue()); } System.out.println("done"); }
private void updateStartAndGoalOffsetErrorTranslation() { //Translation stateEstimatorReferenceFrame.getTransformToDesiredFrame(stateEstimatorTransform_Translation, worldFrame); stateEstimatorTransform_Translation.setRotationToIdentity(); stateEstimatorPose_Translation.setPose(stateEstimatorTransform_Translation); stateEstimatorReferenceFrame_Translation.setPoseAndUpdate(stateEstimatorPose_Translation); startOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Translation); startOffsetErrorPose.getPosition(updatedStartOffset_Translation); goalOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Translation); goalOffsetErrorPose.getPosition(updatedGoalOffset_Translation); startOffsetTransform_Translation.setTranslationAndIdentityRotation(updatedStartOffset_Translation); offsetBetweenStartAndGoalVector_Translation.sub(updatedGoalOffset_Translation, updatedStartOffset_Translation); goalTranslationRawX.set(offsetBetweenStartAndGoalVector_Translation.getX()); goalTranslationRawY.set(offsetBetweenStartAndGoalVector_Translation.getY()); goalTranslationRawZ.set(offsetBetweenStartAndGoalVector_Translation.getZ()); goalTranslationWithDeadzoneX.update(); goalTranslationWithDeadzoneY.update(); goalTranslationWithDeadzoneZ.update(); updatedGoalOffsetWithDeadzone_Translation.setX(updatedStartOffset_Translation.getX() + goalTranslationWithDeadzoneX.getDoubleValue()); updatedGoalOffsetWithDeadzone_Translation.setY(updatedStartOffset_Translation.getY() + goalTranslationWithDeadzoneY.getDoubleValue()); updatedGoalOffsetWithDeadzone_Translation.setZ(updatedStartOffset_Translation.getZ() + goalTranslationWithDeadzoneZ.getDoubleValue()); goalOffsetTransform_Translation.setTranslationAndIdentityRotation(updatedGoalOffsetWithDeadzone_Translation); }
public static void main(String[] args) { YoVariableRegistry registry = new YoVariableRegistry("test"); YoDouble deadzoneSize = new YoDouble("deadzoneSize", registry); YoDouble input = new YoDouble("input", registry); deadzoneSize.set(2.0); DeadzoneYoVariable testDeadzone = new DeadzoneYoVariable("testDeadZone", input,deadzoneSize,registry); for(int i = -50; i < 51; i++) { input.set((double)i); testDeadzone.update(); System.out.println("//////////////////////////"); System.out.println("uncorrected = " + (double)i); System.out.println("corrected = " + testDeadzone.getDoubleValue()); } System.out.println("done"); }
private void updateStartAndGoalOffsetErrorRotation() { //Rotation stateEstimatorReferenceFrame.getTransformToDesiredFrame(stateEstimatorTransform_Rotation, worldFrame); stateEstimatorTransform_Rotation.zeroTranslation(); stateEstimatorPose_Rotation.setPose(stateEstimatorTransform_Rotation); stateEstimatorReferenceFrame_Rotation.setPoseAndUpdate(stateEstimatorPose_Rotation); startOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Rotation); startOffsetErrorPose.getOrientation(updatedStartOffset_Rotation_quat); startOffsetErrorPose.changeFrame(worldFrame); startOffsetErrorPose.getOrientationIncludingFrame(updatedStartOffset_Rotation); goalOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Rotation); goalOffsetErrorPose.getOrientation(updatedGoalOffset_Rotation_quat); goalOffsetErrorPose.changeFrame(worldFrame); goalOffsetErrorPose.getOrientationIncludingFrame(updatedGoalOffset_Rotation); startOffsetTransform_Rotation.setRotationAndZeroTranslation(updatedStartOffset_Rotation_quat); offsetBetweenStartAndGoal_Rotation.setOrientationFromOneToTwo(updatedGoalOffset_Rotation, updatedStartOffset_Rotation); offsetBetweenStartAndGoal_Rotation.getYawPitchRoll(temporaryYawPitchRoll); goalYawRaw.set(temporaryYawPitchRoll[0]); goalYawWithDeadZone.update(); RotationTools.convertYawPitchRollToQuaternion(goalYawWithDeadZone.getDoubleValue(), temporaryYawPitchRoll[1], temporaryYawPitchRoll[2], updatedGoalOffsetWithDeadZone_Rotation_quat); updatedGoalOffsetWithDeadZone_Rotation_quat.mul(updatedStartOffset_Rotation_quat, updatedGoalOffsetWithDeadZone_Rotation_quat); updatedGoalOffsetWithDeadZone_Rotation.set(updatedGoalOffsetWithDeadZone_Rotation_quat); goalOffsetTransform_Rotation.setRotationAndZeroTranslation(updatedGoalOffsetWithDeadZone_Rotation_quat); }
double deadzone = 2.0; deadzoneSize.set(deadzone); DeadzoneYoVariable testDeadzone = new DeadzoneYoVariable("testDeadZone", input , deadzoneSize, registry); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), 0.0, 1e-14)); input.set(deadzone + verySmallStep); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), verySmallStep, 1e-14)); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), 0.0, 1e-14)); input.set(-deadzone - verySmallStep); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), -verySmallStep, 1e-14)); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), valueToBeCorrected + deadzone, 1e-14)); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), 0.0, 1e-14)); testDeadzone.update(); assertTrue(MathTools.epsilonEquals(testDeadzone.getDoubleValue(), valueToBeCorrected - deadzone, 1e-14));
public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, ReferenceFrame referenceFrame) { DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), deadzoneSize, registry); DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, referenceFrame); return ret; }
public void update() { if (inputVariable == null) throw new NullPointerException("DeadzoneYoVariable must be constructed with a non null " + "input variable to call update(), otherwise use update(double)"); update(inputVariable.getDoubleValue()); }
public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble deadzoneSize, YoFrameTuple3D rawTuple) { DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), rawTuple.getYoZ(), deadzoneSize, registry); DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, rawTuple.getReferenceFrame()); return ret; }
public void update() { if (inputVariable == null) throw new NullPointerException("DeadzoneYoVariable must be constructed with a non null " + "input variable to call update(), otherwise use update(double)"); update(inputVariable.getDoubleValue()); }
public static DeadzoneYoFrameVector createDeadzoneYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable deadzoneSize, YoFrameTuple<?, ?> rawTuple) { DeadzoneYoVariable x = new DeadzoneYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), rawTuple.getYoX(), deadzoneSize, registry); DeadzoneYoVariable y = new DeadzoneYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), rawTuple.getYoY(), deadzoneSize, registry); DeadzoneYoVariable z = new DeadzoneYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), rawTuple.getYoZ(), deadzoneSize, registry); DeadzoneYoFrameVector ret = new DeadzoneYoFrameVector(x, y, z, rawTuple.getReferenceFrame()); return ret; }
goalTranslationRawZ = new DoubleYoVariable("goalTranslationRawZ", registry); goalTranslationWithDeadzoneX = new DeadzoneYoVariable("goalTranslationWithDeadzoneX", goalTranslationRawX, xDeadzoneSize, registry); goalTranslationWithDeadzoneY = new DeadzoneYoVariable("goalTranslationWithDeadzoneY", goalTranslationRawY, yDeadzoneSize, registry); goalTranslationWithDeadzoneZ = new DeadzoneYoVariable("goalTranslationWithDeadzoneZ", goalTranslationRawZ, zDeadzoneSize, registry); goalYawWithDeadZone = new DeadzoneYoVariable("goalYawWithDeadZone", goalYawRaw, yawDeadzoneSize, registry);