public void getNormalizedFrameVector(FrameVector2d frameVector2dToPack) { frameVector2dToPack.setToZero(referenceFrame); frameVector2dToPack.set(line.normalizedVector); }
public void getFrameVector(FrameVector2d vectorToPack) { vectorToPack.setToZero(referenceFrame); vectorToPack.sub(lineSegment.getEndpoints()[1], lineSegment.getEndpoints()[0]); }
private void computeTwoCMPOffsets(ArrayList<YoFramePoint2d> upcomingFootstepLocations, int numberOfFootstepsToConsider) { for (int i = 0; i < numberOfFootstepsToConsider; i++) { FrameVector2d entryOffset = entryOffsets.get(i); FrameVector2d exitOffset = exitOffsets.get(i); entryOffset.setToZero(worldFrame); exitOffset.setToZero(worldFrame); entryOffset.setByProjectionOntoXYPlane(referenceCMPsCalculator.getEntryCMPs().get(i + 1).getFrameTuple()); exitOffset.setByProjectionOntoXYPlane(referenceCMPsCalculator.getExitCMPs().get(i + 1).getFrameTuple()); entryOffset.sub(upcomingFootstepLocations.get(i).getFrameTuple2d()); exitOffset.sub(upcomingFootstepLocations.get(i).getFrameTuple2d()); } }
public void getCMPFeedbackDifference(FrameVector2d cmpFeedbackDifferenceToPack) { cmpFeedbackDifferenceToPack.setToZero(worldFrame); cmpFeedbackDifferenceToPack.setX(feedbackDeltaSolution.get(0, 0)); cmpFeedbackDifferenceToPack.setY(feedbackDeltaSolution.get(1, 0)); }
public static void computeCapturePointVelocity(FrameVector2d capturePointVelocityToPack, FrameVector2d centerOfMassVelocityInWorld, FrameVector2d centerOfMassAccelerationInWorld, double omega0) { centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassAccelerationInWorld.checkReferenceFrameMatch(worldFrame); capturePointVelocityToPack.setToZero(worldFrame); capturePointVelocityToPack.set(centerOfMassAccelerationInWorld); capturePointVelocityToPack.scale(1.0 / omega0); capturePointVelocityToPack.add(centerOfMassVelocityInWorld); }
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; } }
icpToCMPVector.setToZero(projectionArea.getReferenceFrame()); icpToCMPVector.sub(desiredCMP, capturePoint); icpToCandidateVector.setToZero(projectionArea.getReferenceFrame()); icpToCandidateVector.sub(candidate, capturePoint); finalICPToICPVector.setToZero(projectionArea.getReferenceFrame()); finalICPToICPVector.sub(capturePoint, finalCapturePoint); icpToCandidateVector.setToZero(projectionArea.getReferenceFrame()); icpToCandidateVector.sub(candidate, capturePoint); finalICPToICPVector.setToZero(projectionArea.getReferenceFrame()); finalICPToICPVector.sub(capturePoint, finalCapturePoint); icpToCandidateVector.setToZero(projectionArea.getReferenceFrame()); icpToCandidateVector.sub(vertex, capturePoint);
private void getTransformedFeedbackGains(FrameVector2d feedbackGainsToPack) { double epsilonZeroICPVelocity = 1e-5; if (desiredICPVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) { icpVelocityDirectionFrame.setXAxis(desiredICPVelocity); RigidBodyTransform transform = icpVelocityDirectionFrame.getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotationTranspose.transpose(); transformTranspose.setRotation(rotationTranspose); gainsMatrix.setZero(); gainsMatrix.setElement(0, 0, 1.0 + feedbackParallelGain.getDoubleValue()); gainsMatrix.setElement(1, 1, 1.0 + feedbackOrthogonalGain.getDoubleValue()); gainsMatrixTransformed.set(rotation); gainsMatrixTransformed.mul(gainsMatrix); gainsMatrixTransformed.mul(rotationTranspose); feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.setX(gainsMatrixTransformed.getElement(0, 0)); feedbackGainsToPack.setY(gainsMatrixTransformed.getElement(1, 1)); } else { feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.set(feedbackOrthogonalGain.getDoubleValue(), feedbackOrthogonalGain.getDoubleValue()); } yoFeedbackGains.set(feedbackGainsToPack); }
private void getTransformedWeights(FrameVector2d weightsToPack, double forwardWeight, double lateralWeight) { RigidBodyTransform transform = contactableFeet.get(supportSide.getEnumValue()).getSoleFrame().getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotation.transpose(); transformTranspose.setRotation(rotationTranspose); weightsMatrix.setZero(); weightsMatrix.setElement(0, 0, forwardWeight); weightsMatrix.setElement(1, 1, lateralWeight); weightsMatrixTransformed.set(rotation); weightsMatrixTransformed.mul(weightsMatrix); weightsMatrixTransformed.mul(rotationTranspose); weightsToPack.setToZero(worldFrame); weightsToPack.setX(weightsMatrixTransformed.getElement(0, 0)); weightsToPack.setY(weightsMatrixTransformed.getElement(1, 1)); }
angularVelocity2d.setToZero(soleFrame); lineOfRotationInSoleFrame.setIncludingFrame(soleFrame, 0.0, 0.0, 1.0, 0.0);
icpOffsetForFreezing.setToZero(); desiredICPToModify.changeFrame(worldFrame); desiredICPVelocityToModify.changeFrame(worldFrame);
comXYAcceleration.setToZero(desiredICPVelocity.getReferenceFrame());
copError.setToZero(planeFrame); copError.sub(copDesired, copActual); yoCoPError.get(robotSide).set(copError);