private ConstantSideFourBarCalculatorWithDerivatives createFourBarCalculator(FrameVector2d vectorBCProjected, FrameVector2d vectorCDProjected, FrameVector2d vectorDAProjected, FrameVector2d vectorABProjected) { double masterLinkAB = vectorABProjected.length(); double BC = vectorBCProjected.length(); double CD = vectorCDProjected.length(); double DA = vectorDAProjected.length(); if (DEBUG) { System.out.println("\nLink length debugging: \n"); System.out.println("masterLinkAB BC CD DA : " + masterLinkAB + ", " + BC + ", " + CD + ", " + DA); } ConstantSideFourBarCalculatorWithDerivatives fourBarCalculator = new ConstantSideFourBarCalculatorWithDerivatives(DA, masterLinkAB, BC, CD); return fourBarCalculator; }
public double length() { return getFrameTuple2d().length(); }
public void clipMaxLength(double maxLength) { if (maxLength < 1e-7) { this.set(0.0, 0.0); return; } double length = this.length(); if (length < maxLength) return; scale(maxLength / length); }
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())); }
private void rateLimitCMP(FramePoint2d cmp, FramePoint2d cmpPreviousValue, FramePoint2d perfectCMP, FramePoint2d previousPerfectCMP) { if (feedbackPartMaxRate.getDoubleValue() < 1.0e-3) return; cmpError.setToZero(cmp.getReferenceFrame()); cmpError.sub(cmp, perfectCMP); cmpErrorPreviousValue.setToZero(cmp.getReferenceFrame()); cmpErrorPreviousValue.sub(cmpPreviousValue, previousPerfectCMP); cmpErrorDifference.sub(cmpError, cmpErrorPreviousValue); double errorDifferenceMagnitude = cmpErrorDifference.length(); double errorDifferenceMax = controlDT * feedbackPartMaxRate.getDoubleValue(); if (errorDifferenceMagnitude > errorDifferenceMax) cmpErrorDifference.scale(errorDifferenceMax / errorDifferenceMagnitude); cmpError.add(cmpErrorPreviousValue, cmpErrorDifference); cmp.add(perfectCMP, cmpError); }
public boolean isRotating(FramePoint2d cop, FramePoint2d desiredCop, FrameLine2d lineOfRotation) { cop.checkReferenceFrameMatch(soleFrame); desiredCop.checkReferenceFrameMatch(soleFrame); lineOfRotation.checkReferenceFrameMatch(soleFrame); if (!lineOfRotation.isPointOnLine(cop)) return false; copError2d.setToZero(soleFrame); copError2d.sub(desiredCop, cop); yoCopError.set(copError2d); perpendicularCopError.set(lineOfRotation.distance(desiredCop)); boolean errorAboveThreshold = perpendicularCopError.getDoubleValue() >= perpendicluarCopErrorThreshold.getDoubleValue(); perpendicularCopErrorAboveThreshold.set(errorAboveThreshold); double acos = perpendicularCopError.getDoubleValue() / copError2d.length(); angleBetweenCopErrorAndLine.set(Math.acos(Math.abs(acos))); boolean angleInBounds = angleBetweenCopErrorAndLine.getDoubleValue() <= angleThreshold.getDoubleValue(); angleOkay.set(angleInBounds); boolean correctSide = lineOfRotation.isPointOnRightSideOfLine(desiredCop); desiredCopOnCorrectSide.set(correctSide); return errorAboveThreshold && angleInBounds && correctSide; } }
double stepLength = desiredOffsetFromSupport.length(); if (stepLength > maxStepLength.getDoubleValue())
copError.sub(copDesired, copActual); yoCoPError.get(robotSide).set(copError); yoCoPErrorMagnitude.get(robotSide).set(copError.length());