public VirtualChainDataPoint(
      ReferenceFrame baseFrame,
      ArrayList<ReferenceFrame> referenceFrames,
      FramePoint2d centerOfMassProjection)
        //   public VirtualChainDataPoint(ReferenceFrame baseFrame, ArrayList<ReferenceFrame>
        // referenceFrames, FramePoint centerOfMassProjection)
      {
    centerOfMassProjection.checkReferenceFrameMatch(baseFrame);

    if (!centerOfMassProjection.getReferenceFrame().isZupFrame()) {
      throw new RuntimeException("centerOfMassProjection is Not a ZUp reference frame!");
    }

    this.centerOfMassProjection = new FramePoint2d(centerOfMassProjection);
    //      centerOfMassProjection.changeFrame(baseFrame);

    rotationMatrices = new ArrayList<Matrix3d>();

    for (int i = 0; i < referenceFrames.size(); i++) {
      ReferenceFrame virtualChainFrame = referenceFrames.get(i);

      RigidBodyTransform transform3D = virtualChainFrame.getTransformToDesiredFrame(baseFrame);

      Matrix3d rotationMatrix = new Matrix3d();
      transform3D.get(rotationMatrix);

      rotationMatrices.add(rotationMatrix);
    }
  }
  public double resolveCenterOfPressureAndNormalTorque(
      FramePoint2d centerOfPressureToPack,
      SpatialForceVector spatialForceVector,
      ReferenceFrame centerOfPressurePlaneFrame) {
    // First resolve the wrench at the plane origin:
    wrenchResolvedOnPlane.set(spatialForceVector);
    wrenchResolvedOnPlane.changeFrame(centerOfPressurePlaneFrame);

    wrenchResolvedOnPlane.getAngularPart(torqueAtZeroInPlaneFrame);
    wrenchResolvedOnPlane.getLinearPart(forceInPlaneFrame);

    double fz = forceInPlaneFrame.getZ();

    double vector12x = Double.NaN;
    double vector12y = Double.NaN;

    double normalTorqueAtCenterOfPressure;
    if (fz > 1e-7) {
      // with sufficient normal force
      vector12x = -1.0 / fz * torqueAtZeroInPlaneFrame.getY();
      vector12y = 1.0 / fz * torqueAtZeroInPlaneFrame.getX();
      normalTorqueAtCenterOfPressure =
          torqueAtZeroInPlaneFrame.getZ()
              - vector12x * forceInPlaneFrame.getY()
              + vector12y * forceInPlaneFrame.getX();
    } else {
      // without normal force
      normalTorqueAtCenterOfPressure = torqueAtZeroInPlaneFrame.getZ();
    }

    centerOfPressureToPack.setIncludingFrame(centerOfPressurePlaneFrame, vector12x, vector12y);
    return normalTorqueAtCenterOfPressure;
  }
  private void computeDistanceAndAngleToDestination(
      ReferenceFrame supportAnkleZUpFrame,
      RobotSide swingLegSide,
      FramePoint2d desiredDestination) {
    FramePoint2d destinationInAnkleFrame = new FramePoint2d(desiredDestination);
    destinationInAnkleFrame.changeFrame(supportAnkleZUpFrame);

    FramePoint2d squaredUpMidpointInAnkleFrame =
        new FramePoint2d(
            supportAnkleZUpFrame,
            0.0,
            swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue() / 2.0));

    FrameVector2d midpointToDestination = new FrameVector2d(destinationInAnkleFrame);
    midpointToDestination.sub(squaredUpMidpointInAnkleFrame);

    distanceToDestination.set(midpointToDestination.length());
    angleToDestination.set(Math.atan2(midpointToDestination.getY(), midpointToDestination.getX()));
  }
 @Override
 public void computeAndPackCoP(FramePoint2d copToPack) {
   copToPack.setToNaN(getMeasurementFrame());
 }