public double resolveCenterOfPressureAndNormalTorque(
      FramePoint 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, 0.0);
    return normalTorqueAtCenterOfPressure;
  }
 public void computeCentroidalMomentumRate(
     InverseDynamicsJoint[] jointsToOptimizeFor, DenseMatrix64F jointAccelerations) {
   DenseMatrix64F centroidalMomentumMatrixPart =
       getCentroidalMomentumMatrixPart(jointsToOptimizeFor);
   CommonOps.mult(centroidalMomentumMatrixPart, jointAccelerations, hdot);
   CommonOps.addEquals(hdot, adotV);
   centroidalMomentumRate.set(centerOfMassFrame, hdot);
 }
  public void solve(SpatialForceVector momentumRateOfChange) {
    T.reshape(SpatialMotionVector.SIZE, 0);
    alpha1.reshape(T.getNumCols(), 1);
    N.reshape(SpatialForceVector.SIZE, SpatialForceVector.SIZE);
    beta1.reshape(N.getNumCols(), 1);

    CommonOps.setIdentity(N);
    momentumRateOfChange.packMatrix(beta1);

    solve(T, alpha1, N, beta1);
  }
 public void getRateOfChangeOfMomentum(SpatialForceVector rateOfChangeOfMomentumToPack) {
   rateOfChangeOfMomentumToPack.set(centroidalMomentumMatrix.getReferenceFrame(), hdot);
 }