public static void transformFrameVector2dIntoRowVector(DenseMatrix64F matrix, FrameVector2d frameVector) { matrix.set(0, 0, frameVector.getX()); matrix.set(0, 1, frameVector.getY()); }
public static void transformFrameVector2dIntoColumnVector(DenseMatrix64F matrix, FrameVector2d frameVector) { matrix.set(0, 0, frameVector.getX()); matrix.set(1, 0, frameVector.getY()); }
@Override public void setFinalHeadingTarget(FrameVector2d finalHeadingTarget) { finalHeadingTarget.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setFinalHeadingTargetAngle(Math.atan2(finalHeadingTarget.getY(), finalHeadingTarget.getX())); }
@Override public void setFinalHeadingTarget(FrameVector2d finalHeadingTarget) { finalHeadingTarget.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setFinalHeadingTargetAngle(Math.atan2(finalHeadingTarget.getY(), finalHeadingTarget.getX())); }
public void update(FrameVector2d vector2dUnfiltered) { x.update(vector2dUnfiltered.getX()); y.update(vector2dUnfiltered.getY()); }
private void scaleFeedbackWeightWithGain() { getTransformedWeights(feedbackWeights, feedbackForwardWeight.getDoubleValue(), feedbackLateralWeight.getDoubleValue()); scaledFeedbackWeight.set(feedbackWeights); if (scaleFeedbackWeightWithGain.getBooleanValue()) { getTransformedFeedbackGains(feedbackGains); double alpha = Math.sqrt(Math.pow(feedbackGains.getX(), 2) + Math.pow(feedbackGains.getY(), 2)); scaledFeedbackWeight.scale(1.0 / alpha); } }
public void update(FrameVector2d vector2dUnfiltered) { checkReferenceFrameMatch(vector2dUnfiltered); x.update(vector2dUnfiltered.getX()); y.update(vector2dUnfiltered.getY()); }
public void update(FrameVector2d vector2dUnfiltered) { checkReferenceFrameMatch(vector2dUnfiltered); x.update(vector2dUnfiltered.getX()); y.update(vector2dUnfiltered.getY()); }
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 setTranslationAndUpdate(FrameVector2d translation) { translation.checkReferenceFrameMatch(parentFrame); this.translation.setX(translation.getX()); this.translation.setY(translation.getY()); this.update(); }
private void setFeedbackConditions() { getTransformedFeedbackGains(feedbackGains); double dynamicRelaxationWeight = this.dynamicRelaxationWeight.getDoubleValue(); if (!localUseStepAdjustment) dynamicRelaxationWeight = dynamicRelaxationWeight / dynamicRelaxationDoubleSupportWeightModifier; solver.setFeedbackConditions(scaledFeedbackWeight.getX(), scaledFeedbackWeight.getY(), feedbackGains.getX(), feedbackGains.getY(), dynamicRelaxationWeight); }
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; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
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())); }
public void setDesiredCapturePointState(FramePoint2d currentDesiredCapturePointPosition, FrameVector2d currentDesiredCapturePointVelocity) { // Do not set the Z to zero! desiredCapturePointPosition.checkReferenceFrameMatch(currentDesiredCapturePointPosition); desiredCapturePointPosition.setX(currentDesiredCapturePointPosition.getX()); desiredCapturePointPosition.setY(currentDesiredCapturePointPosition.getY()); desiredCapturePointVelocity.checkReferenceFrameMatch(currentDesiredCapturePointVelocity); desiredCapturePointVelocity.setX(currentDesiredCapturePointVelocity.getX()); desiredCapturePointVelocity.setY(currentDesiredCapturePointVelocity.getY()); }
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; }
public boolean isTransitionToSingleSupportSafe(RobotSide transferToSide) { getICPError(icpError2d); ReferenceFrame leadingAnkleZUpFrame = bipedSupportPolygons.getAnkleZUpFrames().get(transferToSide); icpError2d.changeFrame(leadingAnkleZUpFrame); double ellipticErrorSquared = MathTools.square(icpError2d.getX() / maxICPErrorBeforeSingleSupportX.getDoubleValue()) + MathTools.square(icpError2d.getY() / maxICPErrorBeforeSingleSupportY.getDoubleValue()); boolean closeEnough = ellipticErrorSquared < 1.0; return closeEnough; }
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); }
@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); } };