How to use

getFrameTuple

method

in

us.ihmc.robotics.math.filters.AlphaFilteredYoFrameVector

/**
* Estimates the pelvis position and linear velocity using the leg kinematics
* @param trustedFoot is the foot used to estimates the pelvis state
* @param numberOfTrustedSides is only one or both legs used to estimate the pelvis state
*/
private void updatePelvisWithKinematics(RigidBody trustedFoot, int numberOfTrustedFeet)
{
 double scaleFactor = 1.0 / numberOfTrustedFeet;
 footToRootJointPositions.get(trustedFoot).getFrameTuple(tempPosition);
 tempPosition.scale(scaleFactor);
 rootJointPosition.add(tempPosition);
 footPositionsInWorld.get(trustedFoot).getFrameTuple(tempPosition);
 tempPosition.scale(scaleFactor);
 rootJointPosition.add(tempPosition);
 YoFramePoint rootJointPositionPerFoot = rootJointPositionsPerFoot.get(trustedFoot);
 rootJointPositionPerFoot.set(footPositionsInWorld.get(trustedFoot));
 rootJointPositionPerFoot.add(footToRootJointPositions.get(trustedFoot));
 footVelocitiesInWorld.get(trustedFoot).getFrameTupleIncludingFrame(tempFrameVector);
 tempFrameVector.scale(scaleFactor * alphaRootJointLinearVelocityNewTwist.getDoubleValue());
 rootJointLinearVelocityNewTwist.sub(tempFrameVector);
}

public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint)
{
 filteredYoAngularMomentum.getFrameTuple(angularMomentum);
 ReferenceFrame comFrame = angularMomentum.getReferenceFrame();
 localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint);
 localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame);
 double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity);
 adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX());
 adjustedDesiredCapturePoint.scale(scaleFactor);
 adjustedDesiredCapturePoint.add(localDesiredCapturePoint);
 adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame());
}