Examples with Translation2d com.team254.lib.geometry.Translation2d used on opensource projects

Example 1 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class AdaptivePurePursuitController method getLength.

/**
 * Gives the length of the arc joining the lookahead point and robot pose (assuming forward motion).
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return the length of the arc joining the lookahead point and robot pose
 */
public static double getLength(Pose2d pose, Translation2d point) {
    final double radius = getRadius(pose, point);
    final Translation2d center = getCenter(pose, point);
    return getLength(pose, point, center, radius);
}

Example 2 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class AdaptivePurePursuitController method getLength.

public static double getLength(Pose2d pose, Translation2d point, Translation2d center, double radius) {
    if (radius < kReallyBigNumber) {
        final Translation2d centerToPoint = new Translation2d(center, point);
        final Translation2d centerToPose = new Translation2d(center, pose.getTranslation());
        // If the point is behind pose, we want the opposite of this angle. To determine if the point is behind,
        // check the sign of the cross-product between the normal vector and the vector from pose to point.
        final boolean behind = Math.signum(Translation2d.cross(pose.getRotation().normal().toTranslation(), new Translation2d(pose.getTranslation(), point))) > 0.0;
        final Rotation2d angle = Translation2d.getAngle(centerToPose, centerToPoint);
        return radius * (behind ? 2.0 * Math.PI - Math.abs(angle.getRadians()) : Math.abs(angle.getRadians()));
    } else {
        return new Translation2d(pose.getTranslation(), point).norm();
    }
}

Example 3 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class PathSegment method getDistanceTravelled.

private double getDistanceTravelled(Translation2d robotPosition) {
    Translation2d pathPosition = getClosestPoint(robotPosition);
    double remainingDist = getRemainingDistance(pathPosition);
    return getLength() - remainingDist;
}

Example 4 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class PathSegment method getClosestPoint.

/**
 * Gets the point on the segment closest to the robot
 *
 * @param position the current position of the robot
 * @return the point on the segment closest to the robot
 */
public Translation2d getClosestPoint(Translation2d position) {
    if (isLine) {
        Translation2d delta = new Translation2d(start, end);
        double u = ((position.x() - start.x()) * delta.x() + (position.y() - start.y()) * delta.y()) / (delta.x() * delta.x() + delta.y() * delta.y());
        if (u >= 0 && u <= 1)
            return new Translation2d(start.x() + u * delta.x(), start.y() + u * delta.y());
        return (u < 0) ? start : end;
    } else {
        Translation2d deltaPosition = new Translation2d(center, position);
        deltaPosition = deltaPosition.scale(deltaStart.norm() / deltaPosition.norm());
        if (Translation2d.cross(deltaPosition, deltaStart) * Translation2d.cross(deltaPosition, deltaEnd) < 0) {
            return center.translateBy(deltaPosition);
        } else {
            Translation2d startDist = new Translation2d(position, start);
            Translation2d endDist = new Translation2d(position, end);
            return (endDist.norm() < startDist.norm()) ? end : start;
        }
    }
}

Example 5 with Translation2d

use of com.team254.lib.geometry.Translation2d in project K-9 by TheGreenMachine.

the class QuinticHermiteSpline method runOptimizationIteration.

/**
 * Runs a single optimization iteration
 */
private static void runOptimizationIteration(List<QuinticHermiteSpline> splines) {
    // can't optimize anything with less than 2 splines
    if (splines.size() <= 1) {
        return;
    }
    ControlPoint[] controlPoints = new ControlPoint[splines.size() - 1];
    double magnitude = 0;
    for (int i = 0; i < splines.size() - 1; ++i) {
        // don't try to optimize colinear points
        if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) {
            continue;
        }
        double original = sumDCurvature2(splines);
        QuinticHermiteSpline temp, temp1;
        temp = splines.get(i);
        temp1 = splines.get(i + 1);
        // holds the gradient at a control point
        controlPoints[i] = new ControlPoint();
        // calculate partial derivatives of sumDCurvature2
        splines.set(i, new QuinticHermiteSpline(temp.x0, temp.x1, temp.dx0, temp.dx1, temp.ddx0, temp.ddx1 + kEpsilon, temp.y0, temp.y1, temp.dy0, temp.dy1, temp.ddy0, temp.ddy1));
        splines.set(i + 1, new QuinticHermiteSpline(temp1.x0, temp1.x1, temp1.dx0, temp1.dx1, temp1.ddx0 + kEpsilon, temp1.ddx1, temp1.y0, temp1.y1, temp1.dy0, temp1.dy1, temp1.ddy0, temp1.ddy1));
        controlPoints[i].ddx = (sumDCurvature2(splines) - original) / kEpsilon;
        splines.set(i, new QuinticHermiteSpline(temp.x0, temp.x1, temp.dx0, temp.dx1, temp.ddx0, temp.ddx1, temp.y0, temp.y1, temp.dy0, temp.dy1, temp.ddy0, temp.ddy1 + kEpsilon));
        splines.set(i + 1, new QuinticHermiteSpline(temp1.x0, temp1.x1, temp1.dx0, temp1.dx1, temp1.ddx0, temp1.ddx1, temp1.y0, temp1.y1, temp1.dy0, temp1.dy1, temp1.ddy0 + kEpsilon, temp1.ddy1));
        controlPoints[i].ddy = (sumDCurvature2(splines) - original) / kEpsilon;
        splines.set(i, temp);
        splines.set(i + 1, temp1);
        magnitude += controlPoints[i].ddx * controlPoints[i].ddx + controlPoints[i].ddy * controlPoints[i].ddy;
    }
    magnitude = Math.sqrt(magnitude);
    // minimize along the direction of the gradient
    // first calculate 3 points along the direction of the gradient
    Translation2d p1, p2, p3;
    // middle point is at the current location
    p2 = new Translation2d(0, sumDCurvature2(splines));
    for (int i = 0; i < splines.size() - 1; ++i) {
        // first point is offset from the middle location by -stepSize
        if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) {
            continue;
        }
        // normalize to step size
        controlPoints[i].ddx *= kStepSize / magnitude;
        controlPoints[i].ddy *= kStepSize / magnitude;
        // move opposite the gradient by step size amount
        splines.get(i).ddx1 -= controlPoints[i].ddx;
        splines.get(i).ddy1 -= controlPoints[i].ddy;
        splines.get(i + 1).ddx0 -= controlPoints[i].ddx;
        splines.get(i + 1).ddy0 -= controlPoints[i].ddy;
        // recompute the spline's coefficients to account for new second derivatives
        splines.get(i).computeCoefficients();
        splines.get(i + 1).computeCoefficients();
    }
    p1 = new Translation2d(-kStepSize, sumDCurvature2(splines));
    for (int i = 0; i < splines.size() - 1; ++i) {
        // last point is offset from the middle location by +stepSize
        if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) {
            continue;
        }
        // move along the gradient by 2 times the step size amount (to return to original location and move by 1
        // step)
        splines.get(i).ddx1 += 2 * controlPoints[i].ddx;
        splines.get(i).ddy1 += 2 * controlPoints[i].ddy;
        splines.get(i + 1).ddx0 += 2 * controlPoints[i].ddx;
        splines.get(i + 1).ddy0 += 2 * controlPoints[i].ddy;
        // recompute the spline's coefficients to account for new second derivatives
        splines.get(i).computeCoefficients();
        splines.get(i + 1).computeCoefficients();
    }
    p3 = new Translation2d(kStepSize, sumDCurvature2(splines));
    // approximate step size to minimize sumDCurvature2 along the gradient
    double stepSize = fitParabola(p1, p2, p3);
    for (int i = 0; i < splines.size() - 1; ++i) {
        if (splines.get(i).getStartPose().isColinear(splines.get(i + 1).getStartPose()) || splines.get(i).getEndPose().isColinear(splines.get(i + 1).getEndPose())) {
            continue;
        }
        // move by the step size calculated by the parabola fit (+1 to offset for the final transformation to find
        // p3)
        controlPoints[i].ddx *= 1 + stepSize / kStepSize;
        controlPoints[i].ddy *= 1 + stepSize / kStepSize;
        splines.get(i).ddx1 += controlPoints[i].ddx;
        splines.get(i).ddy1 += controlPoints[i].ddy;
        splines.get(i + 1).ddx0 += controlPoints[i].ddx;
        splines.get(i + 1).ddy0 += controlPoints[i].ddy;
        // recompute the spline's coefficients to account for new second derivatives
        splines.get(i).computeCoefficients();
        splines.get(i + 1).computeCoefficients();
    }
}