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);
}