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 getFrameVector(FrameVector2d vectorToPack) { vectorToPack.setToZero(referenceFrame); vectorToPack.sub(lineSegment.getEndpoints()[1], lineSegment.getEndpoints()[0]); }
/** FrameVector2d <p/> A normal vector2d associated with a specific reference frame. */ public FrameVector2d(FramePoint2d startFramePoint, FramePoint2d endFramePoint) { this(endFramePoint.referenceFrame, endFramePoint.tuple.getX(), endFramePoint.tuple.getY(), endFramePoint.name); startFramePoint.checkReferenceFrameMatch(endFramePoint); sub(startFramePoint); }
public void getICPError(FrameVector2d icpErrorToPack) { momentumBasedController.getCapturePoint(capturePoint2d); yoDesiredCapturePoint.getFrameTuple2dIncludingFrame(desiredCapturePoint2d); icpErrorToPack.setIncludingFrame(desiredCapturePoint2d); icpErrorToPack.sub(capturePoint2d); }
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 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())); }
/** * 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 FramePose2d getoffsetPoint() { FramePoint2d ballPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), pickUpLocation.getX(), pickUpLocation.getY()); FramePoint2d robotPosition = new FramePoint2d(midZupFrame, 0.0, 0.0); robotPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector2d walkingDirection = new FrameVector2d(ReferenceFrame.getWorldFrame()); walkingDirection.set(ballPosition2d); walkingDirection.sub(robotPosition); walkingDirection.normalize(); double walkingYaw = Math.atan2(walkingDirection.getY(), walkingDirection.getX()); //get a point offset from the ball double x = ballPosition2d.getX() - walkingDirection.getX() * standingDistance; double y = ballPosition2d.getY() - walkingDirection.getY() * standingDistance; double rotationAngle = Math.toRadians(55); //rotate that point around the ball so that the robot stands to the side. double newX = ballPosition2d.getX() + (x - ballPosition2d.getX()) * Math.cos(rotationAngle) - (y - ballPosition2d.getY()) * Math.sin(rotationAngle); double newY = ballPosition2d.getY() + (x - ballPosition2d.getX()) * Math.sin(rotationAngle) + (y - ballPosition2d.getY()) * Math.cos(rotationAngle); FramePose2d poseToWalkTo = new FramePose2d(ReferenceFrame.getWorldFrame(), new Point2d(newX, newY), walkingYaw); return poseToWalkTo; }
private FramePose2d getoffsetPoint() { FramePoint2d ballPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), initialSphereDetectionBehavior.getBallLocation().getX(), initialSphereDetectionBehavior.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()); //get a point offset from the ball double x = ballPosition2d.getX() - walkingDirection.getX() * standingDistance; double y = ballPosition2d.getY() - walkingDirection.getY() * standingDistance; double rotationAngle = Math.toRadians(55); //rotate that point around the ball so that the robot stands to the side. double newX = ballPosition2d.getX() + (x - ballPosition2d.getX()) * Math.cos(rotationAngle) - (y - ballPosition2d.getY()) * Math.sin(rotationAngle); double newY = ballPosition2d.getY() + (x - ballPosition2d.getX()) * Math.sin(rotationAngle) + (y - ballPosition2d.getY()) * Math.cos(rotationAngle); FramePose2d poseToWalkTo = new FramePose2d(worldFrame, new Point2d(newX, newY), walkingYaw); return poseToWalkTo; }
private void sequenceShiftWeight() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitPelvisHomeCommand(false); }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2d vectorToPack, FramePose fromPose, FramePose toPose) { if (fromPose.epsilonEquals(toPose, 1e-7, Double.MAX_VALUE)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2d frameTuple2d = vectorToPack.getFrameTuple2d(); frameTuple2d.setByProjectionOntoXYPlane(toPose.getFramePointCopy()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.setWithoutChecks(frameTuple2d); } }
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; } }
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; }
private void sequenceMediumWarmup() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameVector2d shiftScaleVector = new FrameVector2d(midFeetZUpFrame, 0.1, 0.7); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, shiftScaleVector.getX() * desiredPelvisOffset.getX(), shiftScaleVector.getY() * desiredPelvisOffset.getY(), 0.0); sequenceSquats(); sequenceChestRotations(0.55); //TODO increase/decrease limit? sequencePelvisRotations(0.3); //TODO increase/decrease limit? } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitChestHomeCommand(false); submitPelvisHomeCommand(false); }
@Override protected void setBehaviorInput() { TextToSpeechPacket p1 = new TextToSpeechPacket("Walking To Point Two"); sendPacket(p1); walkToPoint2.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint2d walkPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), walkToPoint2.getX(), walkToPoint2.getY()); FramePoint2d robotPosition = new FramePoint2d(midZupFrame, 0.0, 0.0); robotPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector2d walkingDirection = new FrameVector2d(ReferenceFrame.getWorldFrame()); walkingDirection.set(walkPosition2d); walkingDirection.sub(robotPosition); walkingDirection.normalize(); float walkingYaw = (float) Math.atan2(walkingDirection.getY(), walkingDirection.getX()); Quaternion q = new Quaternion(new float[] {0, 0, walkingYaw}); FramePose poseToWalkTo = new FramePose(ReferenceFrame.getWorldFrame(), new Point3d(walkToPoint2.getX(), walkToPoint2.getY(), 0), JMEDataTypeUtils.jMEQuaternionToVecMathQuat4d(q)); atlasPrimitiveActions.walkToLocationPlannedBehavior.setTarget(poseToWalkTo); } };
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); } }
/** * 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); }
finalDesiredICPToICPDirection.sub(finalDesiredICPLocation); rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(capturePoint, finalDesiredICPToICPDirection);
icpOffsetForFreezing.sub(originalICPToModify);
copError.sub(copDesired, copActual); yoCoPError.get(robotSide).set(copError); yoCoPErrorMagnitude.get(robotSide).set(copError.length());