public void computeBeginningOfStateICPProjection(FramePoint2d beginningOfStateICPProjectionToPack, FramePoint2d beginningOfStateICP) { beginningOfStateICPProjectionToPack.set(beginningOfStateICP); beginningOfStateICPProjectionToPack.scale(footstepRecursionMultiplierCalculator.getInitialICPProjectionMultiplier()); }
public void computeFinalICPRecursion(FramePoint2d finalICPRecursionToPack, int numberOfFootstepsToConsider, boolean useTwoCMPs, boolean isInTransfer, double omega0) { computeFinalICP(numberOfFootstepsToConsider, useTwoCMPs, isInTransfer, omega0); finalICPRecursionToPack.setByProjectionOntoXYPlane(finalICP.getFrameTuple()); finalICPRecursionToPack.scale(footstepRecursionMultiplierCalculator.getFinalICPRecursionMultiplier()); finalICPRecursionToPack.scale(footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier()); }
public void computeCMPOffsetRecursionEffect(FramePoint2d cmpOffsetRecursionEffectToPack, ArrayList<YoFramePoint2d> upcomingFootstepLocations, int numberOfFootstepsToConsider) { computeTwoCMPOffsets(upcomingFootstepLocations, numberOfFootstepsToConsider); cmpOffsetRecursionEffectToPack.setToZero(); for (int i = 0; i < numberOfFootstepsToConsider; i++) { totalOffsetEffect.set(exitOffsets.get(i)); totalOffsetEffect.scale(footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(i)); cmpOffsetRecursionEffectToPack.add(totalOffsetEffect); totalOffsetEffect.set(entryOffsets.get(i)); totalOffsetEffect.scale(footstepRecursionMultiplierCalculator.getCMPRecursionEntryMultiplier(i)); cmpOffsetRecursionEffectToPack.add(totalOffsetEffect); } cmpOffsetRecursionEffectToPack.scale(footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier()); }
/** * This function computes the position of the capture point after the time dt has passed given * the current capture point and the Cop which is assumed to be at constant position over dt. */ public static void predictCapturePoint(FramePoint2d ICP, FramePoint2d CoP, double dt, double omega0, FramePoint2d predictedICPtoPack) { // make sure everything is in the same frame: ICP.checkReferenceFrameMatch(CoP); ICP.checkReferenceFrameMatch(predictedICPtoPack); // CP = (ICP - CoP) * exp(omega0*dt) + CoP predictedICPtoPack.set(ICP); predictedICPtoPack.sub(CoP); predictedICPtoPack.scale(Math.exp(omega0 * dt)); predictedICPtoPack.add(CoP); }
public static void computeCapturePoint(FramePoint2d capturePointToPack, FramePoint2d centerOfMassInWorld, FrameVector2d centerOfMassVelocityInWorld, double omega0) { centerOfMassInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); capturePointToPack.setToZero(worldFrame); capturePointToPack.set(centerOfMassVelocityInWorld); capturePointToPack.scale(1.0 / omega0); capturePointToPack.add(centerOfMassInWorld); }
predictedEndOfStateICP.scale(getFinalICPRecursionMultiplier()); tmpPoint.scale(getStanceEntryCMPProjectionMultiplier()); tmpPoint.scale(getStanceExitCMPProjectionMultiplier()); tmpEntry.scale(getCMPRecursionEntryMultiplier(i)); tmpExit.scale(getCMPRecursionExitMultiplier(i)); referenceICPToPack.scale(currentStateProjectionMultiplier.getPositionMultiplier()); tmpPoint.scale(getRemainingStanceEntryCMPProjectionMultiplier()); referenceICPToPack.add(tmpPoint); tmpPoint.scale(getRemainingStanceExitCMPProjectionMultiplier()); referenceICPToPack.add(tmpPoint); tmpPoint.scale(getRemainingPreviousStanceExitCMPProjectionMultiplier()); referenceICPToPack.add(tmpPoint); tmpPoint.scale(getInitialICPProjectionMultiplier()); referenceICPToPack.add(tmpPoint); tmpPoint.scale(remainingStanceCMPProjectionMultipliers.getRemainingEntryVelocityMultiplier()); referenceICPVelocityToPack.add(tmpPoint); tmpPoint.scale(remainingStanceCMPProjectionMultipliers.getRemainingExitVelocityMultiplier()); referenceICPVelocityToPack.add(tmpPoint);
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); }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
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()); }
private void computeCop() { double force = 0.0; centerOfPressure.setToZero(worldFrame); for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).computeAndPackCoP(tempFootCop2d); if (tempFootCop2d.containsNaN()) continue; footSwitches.get(robotSide).computeAndPackFootWrench(tempFootWrench); double footForce = tempFootWrench.getLinearPartZ(); force += footForce; tempFootCop.setIncludingFrame(tempFootCop2d.getReferenceFrame(), tempFootCop2d.getX(), tempFootCop2d.getY(), 0.0); tempFootCop.changeFrame(worldFrame); tempFootCop.scale(footForce); centerOfPressure.add(tempFootCop.getX(), tempFootCop.getY()); } centerOfPressure.scale(1.0 / force); yoCenterOfPressure.set(centerOfPressure); }
supportCornerPoints.get(i).scale(shiftScaleVector.getX(), shiftScaleVector.getY());
this.soleFrame = soleFrame; gridOrigin.setIncludingFrame(soleFrame, -footLength, -footWidth); gridOrigin.scale(0.5);
desiredCenterOfPressure.scale(0.9);
centroid.scale(1.0e-6); capturePoint.sub(centroid);