vectorCDClosurePoint.sub(jointDClosedLoopPosition, jointCPosition); vectorBCProjectedToPack.setByProjectionOntoXYPlaneIncludingFrame(vectorBC); vectorCDProjectedToPack.setByProjectionOntoXYPlaneIncludingFrame(vectorCD); vectorDAProjectedToPack.setByProjectionOntoXYPlaneIncludingFrame(vectorDA); vectorABProjectedToPack.setByProjectionOntoXYPlaneIncludingFrame(vectorAB); vectorDAClosurePointProjectedToPack.setByProjectionOntoXYPlaneIncludingFrame(vectorDAClosurePoint); vectorCDClosurePointProjectedToPack.setByProjectionOntoXYPlaneIncludingFrame(vectorCDClosurePoint);
public void computeAchievedCMP(FrameVector achievedLinearMomentumRate, FramePoint2d achievedCMPToPack) { if (achievedLinearMomentumRate.containsNaN()) return; centerOfMass2d.setToZero(centerOfMassFrame); centerOfMass2d.changeFrame(worldFrame); achievedCoMAcceleration2d.setByProjectionOntoXYPlaneIncludingFrame(achievedLinearMomentumRate); achievedCoMAcceleration2d.scale(1.0 / totalMass); achievedCoMAcceleration2d.changeFrame(worldFrame); achievedCMPToPack.set(achievedCoMAcceleration2d); achievedCMPToPack.scale(-1.0 / (omega0 * omega0)); achievedCMPToPack.add(centerOfMass2d); }
private void computeCapturePoint() { centerOfMassPosition.setToZero(centerOfMassFrame); if (centerOfMassDataHolder != null) { centerOfMassDataHolder.getCenterOfMassVelocity(centerOfMassVelocity); } else { centerOfMassJacobian.getCenterOfMassVelocity(centerOfMassVelocity); } centerOfMassPosition.changeFrame(worldFrame); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition); centerOfMassVelocity2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassVelocity); CapturePointCalculator.computeCapturePoint(capturePoint2d, centerOfMassPosition2d, centerOfMassVelocity2d, omega0.getDoubleValue()); capturePoint2d.changeFrame(yoCapturePoint.getReferenceFrame()); yoCapturePoint.setXY(capturePoint2d); }