public CentroidalMomentumHandler(
      RigidBody rootBody, ReferenceFrame centerOfMassFrame, YoVariableRegistry parentRegistry) {
    this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootBody);

    this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(rootBody, centerOfMassFrame);
    this.previousCentroidalMomentumMatrix =
        new DenseMatrix64F(
            centroidalMomentumMatrix.getMatrix().getNumRows(),
            centroidalMomentumMatrix.getMatrix().getNumCols());
    yoPreviousCentroidalMomentumMatrix =
        new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
            [previousCentroidalMomentumMatrix.getNumCols()];
    MatrixYoVariableConversionTools.populateYoVariables(
        yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);

    int nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
    this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);

    for (InverseDynamicsJoint joint : jointsInOrder) {
      TIntArrayList listToPackIndices = new TIntArrayList();
      ScrewTools.computeIndexForJoint(jointsInOrder, listToPackIndices, joint);
      int[] indices = listToPackIndices.toArray();
      columnsForJoints.put(joint, indices);
    }

    centroidalMomentumRate = new SpatialForceVector(centerOfMassFrame);
    this.centerOfMassFrame = centerOfMassFrame;

    parentRegistry.addChild(registry);

    double robotMass = TotalMassCalculator.computeSubTreeMass(rootBody);
    this.centroidalMomentumRateTermCalculator =
        new CentroidalMomentumRateTermCalculator(rootBody, centerOfMassFrame, v, robotMass);
  }
  public QuadrupedXGaitStepStream(
      QuadrupedPlanarVelocityInputProvider planarVelocityProvider,
      QuadrupedXGaitSettingsInputProvider xGaitSettingsProvider,
      QuadrupedReferenceFrames referenceFrames,
      double controlDT,
      DoubleYoVariable timestamp,
      YoVariableRegistry parentRegistry) {
    this.planarVelocityProvider = planarVelocityProvider;
    this.xGaitSettingsProvider = xGaitSettingsProvider;
    this.xGaitSettings = new QuadrupedXGaitSettings();
    this.xGaitStepPlanner = new QuadrupedXGaitPlanner();
    this.xGaitPreviewSteps = new ArrayList<>(NUMBER_OF_PREVIEW_STEPS);
    this.xGaitCurrentSteps = new EndDependentList<>();
    for (int i = 0; i < NUMBER_OF_PREVIEW_STEPS; i++) {
      xGaitPreviewSteps.add(new QuadrupedTimedStep());
    }
    for (RobotEnd robotEnd : RobotEnd.values) {
      xGaitCurrentSteps.set(robotEnd, new QuadrupedTimedStep());
    }
    this.supportCentroid = new FramePoint();
    this.supportFrame = referenceFrames.getCenterOfFeetZUpFrameAveragingLowestZHeightsAcrossEnds();
    this.bodyZUpFrame = referenceFrames.getBodyZUpFrame();
    this.worldFrame = ReferenceFrame.getWorldFrame();
    this.controlDT = controlDT;
    this.timestamp = timestamp;
    this.bodyOrientation = new FrameOrientation();
    this.stepSequence =
        new PreallocatedList<>(QuadrupedTimedStep.class, NUMBER_OF_PREVIEW_STEPS + 2);

    if (parentRegistry != null) {
      parentRegistry.addChild(registry);
    }
  }
  public TaskspaceHandControlState(
      String namePrefix,
      HandControlState stateEnum,
      MomentumBasedController momentumBasedController,
      int jacobianId,
      RigidBody base,
      RigidBody endEffector,
      YoVariableRegistry parentRegistry) {
    super(stateEnum);

    name =
        namePrefix
            + FormattingTools.underscoredToCamelCase(this.getStateEnum().toString(), true)
            + "State";
    registry = new YoVariableRegistry(name);

    taskspaceConstraintData.set(base, endEffector);

    this.momentumBasedController = momentumBasedController;
    this.jacobianId = jacobianId;
    this.base = base;
    this.endEffector = endEffector;

    parentRegistry.addChild(registry);
  }
  public UserDesiredHandstepProvider(
      FullHumanoidRobotModel fullRobotModel,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    this.handstepHelper = new HandstepHelper(fullRobotModel);

    userHandstepTakeIt.set(false);
    userHandstepNormal.set(-1.0, 0.0, 0.0);
    userHandstepRobotSide.set(RobotSide.LEFT);

    userDesiredHandstepCoordinateSystem =
        new YoGraphicCoordinateSystem("userHandstepViz", "", parentRegistry, 0.3);

    VariableChangedListener listener =
        new VariableChangedListener() {
          public void variableChanged(YoVariable<?> v) {
            Vector3d position = userHandstepPosition.getVector3dCopy();
            Vector3d surfaceNormal = userHandstepNormal.getVector3dCopy();
            double rotationAngleAboutNormal = userHandstepRotationAboutNormal.getDoubleValue();
            userDesiredHandstepCoordinateSystem.setTransformToWorld(
                HandstepHelper.computeHandstepTransform(
                    false, position, surfaceNormal, rotationAngleAboutNormal));
          }
        };

    yoGraphicsListRegistry.registerYoGraphic(
        "UserDesiredHandstep", userDesiredHandstepCoordinateSystem);

    userHandstepNormal.attachVariableChangedListener(listener);
    userHandstepRotationAboutNormal.addVariableChangedListener(listener);
    userHandstepPosition.attachVariableChangedListener(listener);

    parentRegistry.addChild(registry);
  }
  /**
   * Quadruped version, assumes flat ground
   *
   * @param footName name for yovariable registry and yovariable names
   * @param quadrupedFeet all the feet
   * @param switchZThreshold difference in z between the lowest foot and the foot inquestion before
   *     assuming foot is in contact
   * @param totalRobotWeight
   * @param quadrant the foot in question
   * @param parentRegistry
   */
  public KinematicsBasedFootSwitch(
      String footName,
      QuadrantDependentList<? extends ContactablePlaneBody> quadrupedFeet,
      double switchZThreshold,
      double totalRobotWeight,
      RobotQuadrant quadrant,
      YoVariableRegistry parentRegistry) {
    registry = new YoVariableRegistry(footName + getClass().getSimpleName());
    foot = quadrupedFeet.get(quadrant);

    ContactablePlaneBody acrossBodyFrontFoot =
        quadrupedFeet.get(quadrant.getAcrossBodyFrontQuadrant());
    ContactablePlaneBody acrossBodyHindFoot =
        quadrupedFeet.get(quadrant.getAcrossBodyHindQuadrant());
    ContactablePlaneBody sameSideFoot = quadrupedFeet.get(quadrant.getSameSideQuadrant());
    otherFeet = new ContactablePlaneBody[] {acrossBodyFrontFoot, acrossBodyHindFoot, sameSideFoot};

    this.totalRobotWeight = totalRobotWeight;
    hitGround = new BooleanYoVariable(footName + "hitGround", registry);
    fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
    soleZ = new DoubleYoVariable(footName + "soleZ", registry);
    ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
    this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
    this.switchZThreshold.set(switchZThreshold);

    yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);

    parentRegistry.addChild(registry);
  }
  @Before
  public void setUp() {
    YoVariableRegistry robotRegistry = new YoVariableRegistry("robot");

    registry = new YoVariableRegistry("testRegistry");
    robotRegistry.addChild(registry);

    yoVariable = new DoubleYoVariable("variableOne", registry);
    variableChangedListeners = new ArrayList<TestVariableChangedListener>();
  }
  public ForcePerturbance(
      ForcePerturbable forcePerturbable,
      double magnitude,
      double duration,
      double ballVelocityMagnitudeForViz,
      YoVariableRegistry parentRegistry) {
    this.forcePerturbable = forcePerturbable;
    this.disturbanceMagnitude.set(magnitude);
    this.disturbanceDuration.set(duration);
    this.ballVelocityMagnitude = ballVelocityMagnitudeForViz;

    if (parentRegistry != null) parentRegistry.addChild(registry);
  }
  private ArrayList<YoVariable<?>> createALargeNumberOfVariables(
      Random random, int numberOfVariables) {
    YoVariableRegistry rootRegistry = new YoVariableRegistry("rootRegistry");
    YoVariableRegistry registryOne = new YoVariableRegistry("registryOne");
    YoVariableRegistry registryTwo = new YoVariableRegistry("registryTwo");
    YoVariableRegistry registryThree = new YoVariableRegistry("registryThree");

    rootRegistry.addChild(registryOne);
    registryOne.addChild(registryTwo);
    registryTwo.addChild(registryThree);

    DoubleYoVariable t = new DoubleYoVariable("t", registryThree);
    DoubleYoVariable time = new DoubleYoVariable("time", registryThree);
    t.set(1.1);
    time.set(2.2);

    for (int i = 0; i < numberOfVariables; i++) {
      DoubleYoVariable variable = new DoubleYoVariable("variable" + i, registryThree);
      variable.set(Math.random());
    }

    return rootRegistry.getAllVariablesIncludingDescendants();
  }
  public DiagnosticTaskExecutor(
      String namePrefix, DoubleYoVariable yoTime, YoVariableRegistry parentRegistry) {
    parentRegistry.addChild(registry);

    currentTaskIndex = new IntegerYoVariable(namePrefix + "CurrentTaskIndex", registry);
    numberOfTasksRemaining = new IntegerYoVariable(namePrefix + "TasksRemaining", registry);
    isDone = new BooleanYoVariable(namePrefix + "IsDone", registry);
    hasAborted = new BooleanYoVariable(namePrefix + "HasAborted", registry);
    timeInCurrentTask = new DoubleYoVariable(namePrefix + "TimeInCurrentTask", registry);
    switchTime = new DoubleYoVariable(namePrefix + "SwitchTime", registry);
    this.yoTime = yoTime;

    clear();
  }
  public MomentumSolver3(
      SixDoFJoint rootJoint,
      RigidBody elevator,
      ReferenceFrame centerOfMassFrame,
      TwistCalculator twistCalculator,
      LinearSolver<DenseMatrix64F> jacobianSolver,
      double controlDT,
      YoVariableRegistry parentRegistry) {
    this.rootJoint = rootJoint;
    this.jointsInOrder = ScrewTools.computeSupportAndSubtreeJoints(rootJoint.getSuccessor());

    this.motionConstraintHandler =
        new MotionConstraintHandler("solver3", jointsInOrder, twistCalculator, null, registry);

    this.centroidalMomentumMatrix = new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
    this.previousCentroidalMomentumMatrix =
        new DenseMatrix64F(
            centroidalMomentumMatrix.getMatrix().getNumRows(),
            centroidalMomentumMatrix.getMatrix().getNumCols());
    this.centroidalMomentumMatrixDerivative =
        new DenseMatrix64F(
            centroidalMomentumMatrix.getMatrix().getNumRows(),
            centroidalMomentumMatrix.getMatrix().getNumCols());
    yoPreviousCentroidalMomentumMatrix =
        new DoubleYoVariable[previousCentroidalMomentumMatrix.getNumRows()]
            [previousCentroidalMomentumMatrix.getNumCols()];
    MatrixYoVariableConversionTools.populateYoVariables(
        yoPreviousCentroidalMomentumMatrix, "previousCMMatrix", registry);

    this.controlDT = controlDT;

    nDegreesOfFreedom = ScrewTools.computeDegreesOfFreedom(jointsInOrder);
    this.b = new DenseMatrix64F(SpatialMotionVector.SIZE, 1);
    this.v = new DenseMatrix64F(nDegreesOfFreedom, 1);

    //      nullspaceMotionConstraintEnforcer = new
    // EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));
    equalityConstraintEnforcer =
        new EqualityConstraintEnforcer(LinearSolverFactory.pseudoInverse(true));

    solver = LinearSolverFactory.pseudoInverse(true);

    parentRegistry.addChild(registry);
    reset();
  }
  private WandererSingleThreadedController(
      WandererRobotModel robotModel,
      PriorityParameters priorityParameters,
      RobotVisualizer visualizer,
      WandererController wandererController,
      YoVariableRegistry registry) {
    super(priorityParameters);
    this.state = new WandererState(0.001, registry);
    this.reader = new UDPAcsellStateReader(state);

    this.visualizer = visualizer;

    this.command = new WandererCommand(registry);
    this.controller = wandererController;
    this.outputWriter = new UDPAcsellOutputWriter(command);

    FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
    rootJoint = fullRobotModel.getRootJoint();

    for (WandererJoint joint : WandererJoint.values) {
      OneDoFJoint oneDoFJoint = fullRobotModel.getOneDoFJointByName(joint.getSdfName());
      if (oneDoFJoint == null) {
        throw new RuntimeException("Unknown joint: " + joint.getSdfName());
      }
      jointMap.put(joint, oneDoFJoint);
    }

    this.rawSensors = new RawJointSensorDataHolderMap(fullRobotModel);
    this.controller.setFullRobotModel(fullRobotModel);

    if (controller.getYoVariableRegistry() != null) {
      registry.addChild(controller.getYoVariableRegistry());
    }

    if (visualizer != null) {
      visualizer.setMainRegistry(registry, fullRobotModel, null);
    }

    outputWriter.connect(new WandererNetworkParameters());
  }
  public AxisAngleOrientationController(
      String prefix,
      ReferenceFrame bodyFrame,
      double dt,
      YoOrientationPIDGainsInterface gains,
      YoVariableRegistry parentRegistry) {
    this.dt = dt;
    this.bodyFrame = bodyFrame;
    registry = new YoVariableRegistry(prefix + getClass().getSimpleName());

    if (gains == null) gains = new YoAxisAngleOrientationGains(prefix, registry);

    this.gains = gains;
    proportionalGainMatrix = gains.createProportionalGainMatrix();
    derivativeGainMatrix = gains.createDerivativeGainMatrix();
    integralGainMatrix = gains.createIntegralGainMatrix();

    rotationErrorInBody = new YoFrameVector(prefix + "RotationErrorInBody", bodyFrame, registry);
    rotationErrorCumulated =
        new YoFrameVector(prefix + "RotationErrorCumulated", bodyFrame, registry);
    velocityError = new YoFrameVector(prefix + "AngularVelocityError", bodyFrame, registry);

    proportionalTerm = new FrameVector(bodyFrame);
    derivativeTerm = new FrameVector(bodyFrame);
    integralTerm = new FrameVector(bodyFrame);

    feedbackAngularAction =
        new YoFrameVector(prefix + "FeedbackAngularAction", bodyFrame, registry);
    rateLimitedFeedbackAngularAction =
        RateLimitedYoFrameVector.createRateLimitedYoFrameVector(
            prefix + "RateLimitedFeedbackAngularAction",
            "",
            registry,
            gains.getYoMaximumFeedbackRate(),
            dt,
            feedbackAngularAction);

    parentRegistry.addChild(registry);
  }
  public KinematicsBasedFootSwitch(
      String footName,
      SideDependentList<? extends ContactablePlaneBody> bipedFeet,
      double switchZThreshold,
      double totalRobotWeight,
      RobotSide side,
      YoVariableRegistry parentRegistry) {
    registry = new YoVariableRegistry(footName + getClass().getSimpleName());
    foot = bipedFeet.get(side);
    ContactablePlaneBody oppositeFoot = bipedFeet.get(side.getOppositeSide());
    otherFeet = new ContactablePlaneBody[] {oppositeFoot};
    this.totalRobotWeight = totalRobotWeight;
    hitGround = new BooleanYoVariable(footName + "hitGround", registry);
    fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
    soleZ = new DoubleYoVariable(footName + "soleZ", registry);
    ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
    this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
    this.switchZThreshold.set(switchZThreshold);

    yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);

    parentRegistry.addChild(registry);
  }
  public TwoWaypointSwingGenerator(
      String namePrefix,
      double maxSwingHeight,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
    parentRegistry.addChild(registry);

    stepTime = new DoubleYoVariable(namePrefix + "StepTime", registry);
    timeIntoStep = new DoubleYoVariable(namePrefix + "TimeIntoStep", registry);
    isDone = new BooleanYoVariable(namePrefix + "IsDone", registry);
    trajectoryType =
        new EnumYoVariable<>(namePrefix + "TrajectoryType", registry, TrajectoryType.class);
    swingHeight = new DoubleYoVariable(namePrefix + "SwingHeight", registry);
    swingHeight.set(defaultSwingHeight);

    this.maxSwingHeight = new DoubleYoVariable(namePrefix + "MaxSwingHeight", registry);
    this.maxSwingHeight.set(maxSwingHeight);

    trajectory =
        new Position2dOptimizedSwingTrajectoryGenerator(
            namePrefix, registry, yoGraphicsListRegistry, maxTimeIterations);

    for (int i = 0; i < numberWaypoints; i++) waypointPositions.add(new FramePoint());

    if (yoGraphicsListRegistry != null)
      waypointViz =
          new BagOfBalls(
              numberWaypoints,
              0.02,
              namePrefix + "Waypoints",
              YoAppearance.White(),
              registry,
              yoGraphicsListRegistry);
    else waypointViz = null;
  }
  public CirclePoseTrajectoryGenerator(
      String namePrefix,
      ReferenceFrame trajectoryFrame,
      DoubleProvider trajectoryTimeProvider,
      YoVariableRegistry parentRegistry,
      YoGraphicsListRegistry yoGraphicsListRegistry) {
    this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
    this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
    this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);

    //      this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
    // registry);
    this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
    //      this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
    // registry);

    this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;

    this.trajectoryFrame = trajectoryFrame;

    initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
    initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
    initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
    finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);

    isCurrentAngleBeingAdjusted =
        new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
    maximumAngleTrackingErrorTolerated =
        new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
    maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
    currentControlledFrameRelativeAngle =
        new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
    currentAngleTrackingError =
        new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
    currentAdjustedRelativeAngle =
        new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);

    desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
    currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);

    yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
    yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);

    yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
    yoCurrentVelocity =
        new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
    yoCurrentAcceleration =
        new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);

    yoInitialOrientation =
        new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
    yoFinalOrientation =
        new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);

    yoCurrentOrientation =
        new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
    yoCurrentAngularVelocity =
        new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
    yoCurrentAngularAcceleration =
        new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);

    yoInitialPositionWorld =
        new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
    yoFinalPositionWorld =
        new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
    yoCurrentPositionWorld =
        new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
    yoCurrentAdjustedPositionWorld =
        new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);

    circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
    rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
    rotationAxis.set(0.0, 0.0, 1.0);

    circleFrame =
        new ReferenceFrame("CircleFrame", trajectoryFrame) {
          private static final long serialVersionUID = 9102217353690768074L;

          private final Vector3d localTranslation = new Vector3d();
          private final Vector3d localRotationAxis = new Vector3d();
          private final AxisAngle4d localAxisAngle = new AxisAngle4d();

          @Override
          protected void updateTransformToParent(RigidBodyTransform transformToParent) {
            circleOrigin.get(localTranslation);
            rotationAxis.get(localRotationAxis);
            GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
            transformToParent.set(localAxisAngle, localTranslation);
          }
        };

    tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
    yoTangentialCircleFramePose =
        new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);

    if (this.visualize && yoGraphicsListRegistry != null) {
      final YoGraphicPosition currentPositionViz =
          new YoGraphicPosition(
              namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
      final YoGraphicPosition currentAdjustedPositionViz =
          new YoGraphicPosition(
              namePrefix + "CurrentAdjustedPosition",
              yoCurrentAdjustedPositionWorld,
              0.023,
              YoAppearance.Gold());
      final YoGraphicPosition initialPositionViz =
          new YoGraphicPosition(
              namePrefix + "InitialPosition",
              yoInitialPositionWorld,
              0.02,
              YoAppearance.BlueViolet());
      final YoGraphicPosition finalPositionViz =
          new YoGraphicPosition(
              namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
      final YoGraphicCoordinateSystem tangentialFrameViz =
          new YoGraphicCoordinateSystem(
              namePrefix + "TangentialFrame",
              yoTangentialCircleFramePose,
              0.2,
              YoAppearance.Pink());

      YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
      yoGraphicsList.add(currentPositionViz);
      yoGraphicsList.add(currentAdjustedPositionViz);
      yoGraphicsList.add(initialPositionViz);
      yoGraphicsList.add(finalPositionViz);
      yoGraphicsList.add(tangentialFrameViz);
      yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);

      bagOfBalls =
          new BagOfBalls(
              numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);

      showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
      showViz.addVariableChangedListener(
          new VariableChangedListener() {
            public void variableChanged(YoVariable<?> v) {
              boolean visible = showViz.getBooleanValue();
              currentPositionViz.setVisible(visible);
              currentAdjustedPositionViz.setVisible(visible);
              initialPositionViz.setVisible(visible);
              finalPositionViz.setVisible(visible);
              tangentialFrameViz.setVisible(visible);
              bagOfBalls.setVisible(visible);
            }
          });
      showViz.notifyVariableChangedListeners();
    } else {
      visualize = false;
      bagOfBalls = null;
      showViz = null;
    }

    parentRegistry.addChild(registry);
  }
  @DeployableTestMethod
  @Test(timeout = 300000)
  public void testDataFileWriterAndReader() throws IOException, RepeatDataBufferEntryException {
    int numDataPoints = 10000;

    DataBuffer dataBuffer = new DataBuffer(numDataPoints);

    YoVariableRegistry rootRegistry = new YoVariableRegistry("rootRegistry");
    YoVariableRegistry registryOne = new YoVariableRegistry("registryOne");
    YoVariableRegistry registryTwo = new YoVariableRegistry("registryTwo");
    YoVariableRegistry registryThree = new YoVariableRegistry("registryThree");

    rootRegistry.addChild(registryOne);
    rootRegistry.addChild(registryTwo);
    registryTwo.addChild(registryThree);

    DoubleYoVariable variableOne = new DoubleYoVariable("variableOne", rootRegistry);
    DoubleYoVariable variableTwo = new DoubleYoVariable("variableTwo", rootRegistry);
    DoubleYoVariable variableThree = new DoubleYoVariable("variableThree", rootRegistry);
    DoubleYoVariable variableFour = new DoubleYoVariable("variableFour", registryOne);
    DoubleYoVariable variableFive = new DoubleYoVariable("variableFive", registryTwo);
    BooleanYoVariable variableSix = new BooleanYoVariable("variableSix", rootRegistry);
    IntegerYoVariable variableSeven = new IntegerYoVariable("variableSeven", registryThree);

    dataBuffer.addVariable(variableOne);
    dataBuffer.addVariable(variableTwo);
    dataBuffer.addVariable(variableThree);
    dataBuffer.addVariable(variableFour);
    dataBuffer.addVariable(variableFive);
    dataBuffer.addVariable(variableSix);
    dataBuffer.addVariable(variableSeven);

    for (int i = 0; i < numDataPoints; i++) {
      variableOne.set(Math.random());
      variableTwo.set(Math.random());
      variableThree.set((int) (Math.random() * 100.0));
      variableFour.set((int) (Math.random() * 100.0));
      variableFive.set(Math.random());
      variableSix.set(Math.random() > 0.5);
      variableSeven.set((int) (Math.random() * 1000.0));

      dataBuffer.tickAndUpdate();
    }

    Robot robot = new Robot("testRobot");

    ArrayList<YoVariable<?>> allVariables = rootRegistry.getAllVariablesIncludingDescendants();

    boolean binary = false;
    boolean compress = false;
    boolean spreadsheetFormatted = true;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    spreadsheetFormatted = false;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    binary = true;
    compress = false;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    binary = false;
    compress = true;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);

    binary = true;
    compress = true;
    testDataWriteReadIsTheSame(
        dataBuffer, allVariables, binary, compress, spreadsheetFormatted, robot);
  }
 public FullRobotModelRootJointRewinder(
     FullRobotModel fullRobotModel, YoVariableRegistry parentRegistry) {
   this.fullRobotModel = fullRobotModel;
   parentRegistry.addChild(registry);
 }