private void updateCapturePointError() { tempFramePoint2d.set(yoDesiredCapturePoint.getX(), yoDesiredCapturePoint.getY()); double error = Math.abs(yoCapturePoint.distance(tempFramePoint2d)); // if (error > maxObservedCapturePointError) // { // maxObservedCapturePointError = error; //// System.out.println("TurnValveBehavior: Max Capture Point Error : " + maxObservedCapturePointError); // } icpError.set(error); } }
private void setFeedbackConditions() { getTransformedFeedbackGains(feedbackGains); double dynamicRelaxationWeight = this.dynamicRelaxationWeight.getDoubleValue(); if (!localUseStepAdjustment) dynamicRelaxationWeight = dynamicRelaxationWeight / dynamicRelaxationDoubleSupportWeightModifier; solver.setFeedbackConditions(scaledFeedbackWeight.getX(), scaledFeedbackWeight.getY(), feedbackGains.getX(), feedbackGains.getY(), dynamicRelaxationWeight); }
public void setDesiredCapturePointState(YoFramePoint2d currentDesiredCapturePointPosition, YoFrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
private void submitFootstepConditionsToSolver(int footstepIndex) { getTransformedWeights(footstepWeights, forwardFootstepWeight.getDoubleValue(), lateralFootstepWeight.getDoubleValue()); scaledFootstepWeights.set(footstepWeights); if (localScaleUpcomingStepWeights) scaledFootstepWeights.scale(1.0 / (footstepIndex + 1)); double footstepRecursionMultiplier; if (localUseTwoCMPs) { double entryMutliplier = footstepRecursionMultiplierCalculator.getCMPRecursionEntryMultiplier(footstepIndex); double exitMutliplier = footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(footstepIndex); footstepRecursionMultiplier = entryMutliplier + exitMutliplier; } else { footstepRecursionMultiplier = footstepRecursionMultiplierCalculator.getCMPRecursionExitMultiplier(footstepIndex); } footstepRecursionMultiplier *= footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier(); solver.setFootstepAdjustmentConditions(footstepIndex, footstepRecursionMultiplier, scaledFootstepWeights.getX(), scaledFootstepWeights.getY(), upcomingFootstepLocations.get(footstepIndex).getFrameTuple2d()); }
desiredCoPMatrix.set(0, 0, desiredCoPCommandInSoleFrame.getX()); desiredCoPMatrix.set(1, 0, desiredCoPCommandInSoleFrame.getY()); desiredCoPWeightMatrix.set(0, 0, desiredCoPCommandWeightInSoleFrame.getX());