private FramePose2d getoffsetPoint() { FramePoint2d ballPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), sphereDetectionBehavior.getBallLocation().getX(), sphereDetectionBehavior.getBallLocation().getY()); FramePoint2d robotPosition = new FramePoint2d(midZupFrame, 0.0, 0.0); robotPosition.changeFrame(worldFrame); FrameVector2d walkingDirection = new FrameVector2d(worldFrame); walkingDirection.set(ballPosition2d); walkingDirection.sub(robotPosition); walkingDirection.normalize(); double walkingYaw = Math.atan2(walkingDirection.getY(), walkingDirection.getX()); double x = ballPosition2d.getX() - walkingDirection.getX() * standingDistance; double y = ballPosition2d.getY() - walkingDirection.getY() * standingDistance; FramePose2d poseToWalkTo = new FramePose2d(worldFrame, new Point2d(x, y), walkingYaw); return poseToWalkTo; }
toLeftOfDesiredHeading.setIncludingFrame(desiredHeading.getReferenceFrame(), -desiredHeading.getY(), desiredHeading.getX()); desiredVelocity.changeFrame(desiredHeading.getReferenceFrame()); velocityMagnitudeInHeading.set(desiredVelocity.dot(desiredHeading)); velocityMagnitudeToLeftOfHeading.set(desiredVelocity.dot(toLeftOfDesiredHeading)); FrameVector2d desiredOffsetInPredictedHeadingFrame = new FrameVector2d(); if (desiredVelocityControlModule.getReferenceFrame().equals(desiredHeadingFrame)) desiredVelocity.changeFrame(desiredHeadingFrame); desiredOffsetInPredictedHeadingFrame.changeFrame(predictedHeadingFrame); desiredOffsetInPredictedHeadingFrame.set(desiredVelocity.getX(), desiredVelocity.getY()); desiredOffsetInPredictedHeadingFrame.scale(stepDuration); desiredOffsetInPredictedHeadingFrame.setIncludingFrame(desiredVelocity); desiredOffsetInPredictedHeadingFrame.changeFrame(predictedHeadingFrame); desiredOffsetInPredictedHeadingFrame.scale(stepDuration); FrameVector2d desiredOffsetFromSupport = new FrameVector2d(desiredOffsetInPredictedHeadingFrame); desiredOffsetFromSupport.add(0.0, swingLegSide.negateIfRightSide(inPlaceWidth.getDoubleValue())); desiredOffsetFromSupport.setY(MathTools.clipToMinMax(desiredOffsetFromSupport.getY(), minStepWidth.getDoubleValue(), maxStepWidth.getDoubleValue())); desiredOffsetFromSupport.setY(MathTools.clipToMinMax(desiredOffsetFromSupport.getY(), -maxStepWidth.getDoubleValue(), -minStepWidth.getDoubleValue())); double stepLength = desiredOffsetFromSupport.length(); if (stepLength > maxStepLength.getDoubleValue()) desiredOffsetFromSupport.scale(maxStepLength.getDoubleValue() / stepLength);
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); }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
shiftingVector.rotate90(); if (sideToLookAt == RobotSide.RIGHT) shiftingVector.negate(); double theta = Math.atan2(shiftedLineVector.getY(), shiftedLineVector.getX()); shiftingVector.scale(cellSize.getX() * Math.cos(theta) + cellSize.getY() * Math.sin(theta)); return returnFailure; FrameVector2d intersectionsVector = new FrameVector2d(soleFrame); intersectionsVector.sub(intersections[1], intersections[0]); int yIndex = -1; if (intersectionsVector.dot(shiftedLineVector) > 0.5) double xDirection = Math.signum(shiftingVector.getX()); double yDirection = Math.signum(shiftingVector.getY()); shiftedLineVector.normalize(); if (Math.abs(theta % Math.PI / 2.0) > 0.1) shiftedLineVector.scale(cellSize.getX() / Math.cos(theta)); shiftedLineVector.scale(cellSize.getY() / Math.sin(theta));
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())); }
desiredCapturePoint.changeFrame(worldFrame); finalDesiredCapturePoint.changeFrame(worldFrame); desiredCapturePointVelocity.changeFrame(worldFrame); tempControl.setIncludingFrame(desiredCapturePointVelocity); tempControl.scale(1.0 / omega0); if (desiredCapturePointVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) tempControl.changeFrame(icpVelocityDirectionFrame); tempControl.setX(tempControl.getX() * captureKpParallelToMotion.getDoubleValue()); tempControl.setY(tempControl.getY() * captureKpOrthogonalToMotion.getDoubleValue()); tempControl.changeFrame(desiredCMP.getReferenceFrame()); tempControl.scale(captureKpOrthogonalToMotion.getDoubleValue()); tempICPErrorIntegrated.scale(controlDT); tempICPErrorIntegrated.scale(captureKi.getDoubleValue()); tempControl.add(icpIntegral);
FrameVector2d frameVector2d = new FrameVector2d(); guideLineSegment.getFrameVector(frameVector2d); if (intersectionToUse == intersections[1]) frameVector2d.scale(-1.0); frameVector2d.normalize(); frameVector2d.scale(amountToBeInside); point.setX(point.getX() + frameVector2d.getX()); point.setY(point.getY() + frameVector2d.getY());
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 Vector2dZUpFrame(String string, ReferenceFrame parentFrame) { super(string, parentFrame); xAxis = new FrameVector2d(parentFrame); }
comXYVelocity.setIncludingFrame(comVelocity.getReferenceFrame(), comVelocity.getX(), comVelocity.getY()); if (desiredICPVelocity.containsNaN()) comXYAcceleration.setToZero(desiredICPVelocity.getReferenceFrame()); comXYAcceleration.setIncludingFrame(desiredICPVelocity); comXYAcceleration.sub(comXYVelocity); comXYAcceleration.scale(omega0); // MathTools.square(omega0.getDoubleValue()) * (com.getX() - copX);
/** * 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); }
@Override public void setFinalHeadingTarget(FrameVector2d finalHeadingTarget) { finalHeadingTarget.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setFinalHeadingTargetAngle(Math.atan2(finalHeadingTarget.getY(), finalHeadingTarget.getX())); }
/** * 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); }
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 void addICPOffset(FramePoint2d desiredICPToModify, FrameVector2d desiredICPVelocityToModify) desiredICPVelocityToModify.changeFrame(supportPolygon.getReferenceFrame()); icpOffsetForFreezing.setToZero(); desiredICPToModify.changeFrame(worldFrame); desiredICPVelocityToModify.changeFrame(worldFrame); return; tempICPOffset.setIncludingFrame(supportFrame, desiredICPOffset.getX(), desiredICPOffset.getY()); tempICPOffset.changeFrame(supportFrame); desiredICPToModify.changeFrame(icpOffsetForFreezing.getReferenceFrame()); desiredICPToModify.add(icpOffsetForFreezing); safeSupportPolygonToConstrainICPOffset.orthogonalProjection(desiredICPToModify); icpOffsetForFreezing.setIncludingFrame(desiredICPToModify); icpOffsetForFreezing.sub(originalICPToModify); desiredICPVelocityToModify.changeFrame(worldFrame);
private FramePoint computeDesiredFootPosition(ReferenceFrame upcomingSupportFrame, FrameVector2d desiredOffsetFromSupport) { desiredOffsetFromSupport.changeFrame(upcomingSupportFrame); FramePoint footstepPosition = new FramePoint(upcomingSupportFrame, desiredOffsetFromSupport.getX(), desiredOffsetFromSupport.getY(), 0.0); footstepPosition.changeFrame(worldFrame); return footstepPosition; }
public void checkIfRobotIsFalling(FramePoint2d capturePoint2d, FramePoint2d desiredCapturePoint2d) { updateCombinedPolygon(); if (isUsingNextFootstep.getBooleanValue()) icpDistanceFromFootPolygon.set(combinedFootPolygonWithNextFootstep.distance(capturePoint2d)); else icpDistanceFromFootPolygon.set(combinedFootPolygon.distance(capturePoint2d)); // TODO need to investigate this method, seems to be buggy // boolean isCapturePointCloseToFootPolygon = combinedFootPolygon.isPointInside(capturePoint, icpDistanceFromFootPolygonThreshold.getDoubleValue()); boolean isCapturePointCloseToFootPolygon = icpDistanceFromFootPolygon.getDoubleValue() < icpDistanceFromFootPolygonThreshold.getDoubleValue(); boolean isCapturePointCloseToDesiredCapturePoint = desiredCapturePoint2d.distance(capturePoint2d) < icpDistanceFromFootPolygonThreshold.getDoubleValue(); isRobotFalling.set(!isCapturePointCloseToFootPolygon && !isCapturePointCloseToDesiredCapturePoint); if (isRobotFalling.getBooleanValue()) { tempFallingDirection.set(capturePoint2d); FramePoint2d footCenter = combinedFootPolygon.getCentroid(); tempFallingDirection.changeFrame(ReferenceFrame.getWorldFrame()); footCenter.changeFrame(ReferenceFrame.getWorldFrame()); tempFallingDirection.sub(footCenter); fallingDirection.set(tempFallingDirection); } }