protected FramePoint2d displacePosition(FramePoint2d footstepPosition2d, double forwardHeading, double offsetForward, double offsetLeft) { double xOffset = offsetForward * Math.cos(forwardHeading) - offsetLeft * Math.sin(forwardHeading); double yOffset = offsetForward * Math.sin(forwardHeading) + offsetLeft * Math.cos(forwardHeading); FrameVector2d offsetVector = new FrameVector2d(WORLD_FRAME, xOffset, yOffset); footstepPosition2d.changeFrame(WORLD_FRAME); footstepPosition2d.add(offsetVector); return footstepPosition2d; }
protected FramePoint2d offsetFootstepFromPath(RobotSide currentFootstepSide, FramePoint2d footstepPosition2d, double footHeading, double offsetAmount) { double sideWaysHeading = footHeading + Math.PI / 2.0; FrameVector2d offsetVector = new FrameVector2d(WORLD_FRAME, Math.cos(sideWaysHeading), Math.sin(sideWaysHeading)); offsetVector.scale(currentFootstepSide.negateIfRightSide(offsetAmount)); FramePoint2d footstepPosition = new FramePoint2d(footstepPosition2d); footstepPosition.changeFrame(WORLD_FRAME); footstepPosition.add(offsetVector); return footstepPosition; }
/** * 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 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()); }
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); }
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); }
shiftedLinePoint.add(shiftingVector);
/** * yawAboutPoint * * @param pointToYawAbout FramePoint2d * @param yaw double * @return CartesianPositionFootstep */ public void yawAboutPoint(FramePoint2d pointToYawAbout, FramePoint2d pointToPack, double yaw) { if (temporaryPointForYawing == null) temporaryPointForYawing = new FrameVector2d(this); else temporaryPointForYawing.setIncludingFrame(this); temporaryPointForYawing.sub(pointToYawAbout); temporaryTransformToDesiredFrame.setIdentity(); temporaryTransformToDesiredFrame.setRotationYawAndZeroTranslation(yaw); temporaryPointForYawing.applyTransform(temporaryTransformToDesiredFrame); pointToPack.setIncludingFrame(pointToYawAbout); pointToPack.add(temporaryPointForYawing); }
tmpPoint.scale(getStanceEntryCMPProjectionMultiplier()); predictedEndOfStateICP.add(tmpPoint); predictedEndOfStateICP.add(tmpPoint); tmpExit.set(footstepLocations.get(i).getFrameTuple2d()); tmpEntry.add(entryOffsets.get(i)); tmpExit.add(exitOffsets.get(i)); predictedEndOfStateICP.add(tmpEntry); predictedEndOfStateICP.add(tmpExit); referenceICPToPack.add(tmpPoint); referenceICPToPack.add(tmpPoint); referenceICPToPack.add(tmpPoint); referenceICPToPack.add(tmpPoint);
private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2d cmpToPack, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidOfFootstepToConsider, YoFrameVector2d cmpOffset, double minForwardCMPOffset, double maxForwardCMPOffset) { // First constrain the computed CMP to the given min/max along the x-axis. FramePoint2d footSupportCentroid = footSupportPolygon.getCentroid(); double cmpXOffsetFromCentroid = stepLengthToCMPOffsetFactor.getDoubleValue() * (centroidOfFootstepToConsider.getX() - footSupportCentroid.getX()) + cmpOffset.getX(); cmpXOffsetFromCentroid = MathTools.clipToMinMax(cmpXOffsetFromCentroid, minForwardCMPOffset, maxForwardCMPOffset); cmpToPack.setIncludingFrame(footSupportCentroid); cmpToPack.add(cmpXOffsetFromCentroid, cmpOffset.getY()); // Then constrain the computed CMP to be inside a safe support region tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon); convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges.getDoubleValue(), footSupportPolygon); footSupportPolygon.orthogonalProjection(cmpToPack); }
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 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); }
private void computeEntryCMP(FramePoint entryCMPToPack, RobotSide robotSide, ReferenceFrame soleFrame, FrameConvexPolygon2d footSupportPolygon, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { if (useTwoCMPsPerSupport) { if (centroidInSoleFrameOfPreviousSupportFoot != null) centroidOfFootstepToConsider.setIncludingFrame(centroidInSoleFrameOfPreviousSupportFoot); else centroidOfFootstepToConsider.setToZero(soleFrame); centroidOfFootstepToConsider.changeFrameAndProjectToXYPlane(soleFrame); if (previousExitCMP != null) { previousExitCMP.getFrameTuple2dIncludingFrame(previousExitCMP2d); previousExitCMP2d.changeFrameAndProjectToXYPlane(soleFrame); // Choose the laziest option if (Math.abs(previousExitCMP2d.getX()) < Math.abs(centroidOfFootstepToConsider.getX())) centroidOfFootstepToConsider.set(previousExitCMP2d); } constrainCMPAccordingToSupportPolygonAndUserOffsets(cmp2d, footSupportPolygon, centroidOfFootstepToConsider, entryCMPUserOffsets.get(robotSide), minForwardEntryCMPOffset.getDoubleValue(), maxForwardEntryCMPOffset.getDoubleValue()); } else { cmp2d.setIncludingFrame(footSupportPolygon.getCentroid()); YoFrameVector2d offset = entryCMPUserOffsets.get(robotSide); cmp2d.add(offset.getX(), offset.getY()); } entryCMPToPack.setXYIncludingFrame(cmp2d); entryCMPToPack.changeFrame(worldFrame); }
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); }
/** * This function projects the footstep midpoint in the capture region. * Might be a bit conservative it should be sufficient to slightly overlap the capture region * and the touch-down polygon. */ private void projectFootstepInCaptureRegion(Footstep footstep, FramePoint2d projectionPoint, FrameConvexPolygon2d captureRegion) { projection.setIncludingFrame(projectionPoint); projection.changeFrame(footstep.getParentFrame()); // move the position of the footstep to the capture region centroid nextStep2d.setIncludingFrame(captureRegion.getCentroid()); nextStep2d.changeFrame(footstep.getParentFrame()); // move the position as far away from the projectionPoint as possible direction.setIncludingFrame(nextStep2d); direction.sub(projection); direction.normalize(); direction.scale(10.0); nextStep2d.add(direction); nextStep2d.changeFrame(captureRegion.getReferenceFrame()); captureRegion.orthogonalProjection(nextStep2d); nextStep2d.changeFrame(footstep.getParentFrame()); footstep.setPositionChangeOnlyXY(nextStep2d); calculateTouchdownFootPolygon(footstep, captureRegion.getReferenceFrame(), touchdownFootPolygon); }
/** * Will return a point on a circle around the origin. The point will be in between the given directions and at * a position specified by the alpha value. E.g. an alpha value of 0.5 will result in the point being in the * middle of the given directions. */ public void getPointBetweenVectorsAtDistanceFromOriginCircular(FrameVector2d directionA, FrameVector2d directionB, double alpha, double radius, FramePoint2d midpoint, FramePoint2d pointToPack) { directionA.checkReferenceFrameMatch(directionB.getReferenceFrame()); directionA.checkReferenceFrameMatch(midpoint.getReferenceFrame()); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); double angleBetweenDirections = directionA.angle(directionB); double angleBetweenDirectionsToSetLine = angleBetweenDirections * alpha; rotatedFromA.setToZero(directionA.getReferenceFrame()); rotatedFromA.set(directionA.getX(), directionA.getY(), 0.0); axisAngle.set(negZRotationAxis, angleBetweenDirectionsToSetLine); rotation.setRotation(axisAngle); rotatedFromA.applyTransform(rotation); rotatedFromA.normalize(); rotatedFromA.scale(radius); pointToPack.changeFrame(rotatedFromA.getReferenceFrame()); pointToPack.set(rotatedFromA.getX(), rotatedFromA.getY()); pointToPack.add(midpoint); } }
icpError.sub(solutionHandler.getControllerReferenceICP()); desiredCMP.add(desiredCMPDelta);
desiredCMP.add(tempControl);