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