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

Example 6 with Pose2d

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

the class AdaptivePurePursuitController method getCenter.

/**
 * Gives the center of the circle joining the lookahead point and robot pose
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return center of the circle joining the lookahead point and robot pose
 */
public static Translation2d getCenter(Pose2d pose, Translation2d point) {
    final Translation2d poseToPointHalfway = pose.getTranslation().interpolate(point, 0.5);
    final Rotation2d normal = pose.getTranslation().inverse().translateBy(poseToPointHalfway).direction().normal();
    final Pose2d perpendicularBisector = new Pose2d(poseToPointHalfway, normal);
    final Pose2d normalFromPose = new Pose2d(pose.getTranslation(), pose.getRotation().normal());
    if (normalFromPose.isColinear(perpendicularBisector.normal())) {
        // Special case: center is poseToPointHalfway.
        return poseToPointHalfway;
    }
    return normalFromPose.intersection(perpendicularBisector);
}

Example 7 with Pose2d

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

the class AdaptivePurePursuitController method getDirection.

/**
 * Gives the direction the robot should turn to stay on the path
 *
 * @param pose  robot pose
 * @param point lookahead point
 * @return the direction the robot should turn: -1 is left, +1 is right
 */
public static int getDirection(Pose2d pose, Translation2d point) {
    Translation2d poseToPoint = new Translation2d(pose.getTranslation(), point);
    Translation2d robot = pose.getRotation().toTranslation();
    double cross = robot.x() * poseToPoint.y() - robot.y() * poseToPoint.x();
    // if robot < pose turn left
    return (cross < 0) ? -1 : 1;
}

Example 8 with Pose2d

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

the class GoalTracker method update.

public synchronized void update(double timestamp, List<Pose2d> field_to_goals) {
    // Try to update existing tracks
    for (Pose2d target : field_to_goals) {
        boolean hasUpdatedTrack = false;
        for (GoalTrack track : mCurrentTracks) {
            if (!hasUpdatedTrack) {
                if (track.tryUpdate(timestamp, target)) {
                    hasUpdatedTrack = true;
                }
            } else {
                track.emptyUpdate();
            }
        }
        if (!hasUpdatedTrack) {
            // Add a new track.
            // System.out.println("Created new track");
            mCurrentTracks.add(GoalTrack.makeNewTrack(timestamp, target, mNextId));
            ++mNextId;
        }
    }
    mCurrentTracks.removeIf(track -> !track.isAlive());
}

Example 9 with Pose2d

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

the class TrajectoryUtil method trajectoryFromSplineWaypoints.

public static Trajectory<Pose2dWithCurvature> trajectoryFromSplineWaypoints(final List<Pose2d> waypoints, double maxDx, double maxDy, double maxDTheta) {
    List<QuinticHermiteSpline> splines = new ArrayList<>(waypoints.size() - 1);
    for (int i = 1; i < waypoints.size(); ++i) {
        splines.add(new QuinticHermiteSpline(waypoints.get(i - 1), waypoints.get(i)));
    }
    QuinticHermiteSpline.optimizeSpline(splines);
    return trajectoryFromSplines(splines, maxDx, maxDy, maxDTheta);
}