Examples with Trajectory com.team254.lib_2014.trajectory.Trajectory used on opensource projects

Example 41 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory 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);
}

Example 42 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project K-9 by TheGreenMachine.

the class TrajectoryTest method verifyTrajectory.

private void verifyTrajectory(Trajectory<TimedState<Pose2dWithCurvature>> trajectory, boolean shouldBeReversed) {
    var iterator = new TrajectoryIterator<>(new TimedView<>(trajectory));
    final double dt = 0.05;
    TimedState<Pose2dWithCurvature> prev_left = null;
    while (!iterator.isDone()) {
        TimedState<Pose2dWithCurvature> left_state = iterator.getState();
        assertTrue((shouldBeReversed ? -1.0 : 1.0) * left_state.velocity() >= -kTestEpsilon);
        if (prev_left != null) {
            // Check there are no angle discontinuities.
            // rad
            final double kMaxReasonableChangeInAngle = 0.3;
            Twist2d left_change = Pose2d.log(prev_left.state().getPose().inverse().transformBy(left_state.state().getPose()));
            assertTrue(Math.abs(left_change.dtheta) < kMaxReasonableChangeInAngle);
            if (!Util.epsilonEquals(left_change.dtheta, 0.0)) {
                // Could be a curvature sign change between prev and now, so just check that either matches our
                // expected sign.
                final boolean left_curvature_positive = left_state.state().getCurvature() > kTestEpsilon || prev_left.state().getCurvature() > kTestEpsilon;
                final boolean left_curvature_negative = left_state.state().getCurvature() < -kTestEpsilon || prev_left.state().getCurvature() < -kTestEpsilon;
                final double actual_left_curvature = left_change.dtheta / left_change.dx;
                if (actual_left_curvature < -kTestEpsilon) {
                    assertTrue(left_curvature_negative);
                } else if (actual_left_curvature > kTestEpsilon) {
                    assertTrue(left_curvature_positive);
                }
            }
        }
        iterator.advance(dt);
        prev_left = left_state;
    }
}

Example 43 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Relic_Main by TeamOverdrive.

the class Drive method driveAction.

public void driveAction(double distance, double heading) {
    double curHeading = this.getGyroAngleDegrees();
    double deltaHeading = heading - curHeading;
    double radius = Math.abs(Math.abs(distance) / (deltaHeading * Math.PI / 180.0));
    TrajectoryGenerator.Strategy strategy = TrajectoryGenerator.SCurvesStrategy;
    Trajectory reference = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, // start velocity
    0.0, curHeading, Math.abs(distance), // goal velocity
    0.0, heading);
    Trajectory leftProfile = reference;
    // Copy
    Trajectory rightProfile = reference.copy();
    double faster = (radius + (WHEEL_BASE / 2.0)) / radius;
    double slower = (radius - (WHEEL_BASE / 2.0)) / radius;
    System.out.println("faster " + faster);
    if (deltaHeading > 0) {
        leftProfile.scale(faster);
        rightProfile.scale(slower);
    } else {
        leftProfile.scale(slower);
        rightProfile.scale(faster);
    }
    new FollowPath(linearOpMode, this, new Path("", new Trajectory.Pair(leftProfile, rightProfile)), (distance > 0.0 ? 1.0 : -1.0), heading);
}

Example 44 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Relic_Main by TeamOverdrive.

the class NewTestSplines method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    telemetry.addData("P", Constants.p.getDouble());
    telemetry.addData("D", Constants.d.getDouble());
    telemetry.addData("V", Constants.v.getDouble());
    telemetry.addData("A", Constants.a.getDouble());
    telemetry.update();
    // Path driveOffStone = Line.calculate(defaultTrajectoryConfig,
    // SCurvesStrategy,
    // 0.0,
    // 0.00001,
    // 10,
    // 0.00001);
    // Path fullPath = ScalePath.calculate(driveOffStone,
    // 1.0/Constants.scale.getDouble());
    Trajectory ref = TrajectoryGenerator.generate(defaultTrajectoryConfig, TrajectoryGenerator.SCurvesStrategy, 0, 0, 10, 0, 0.000001);
    // Init our drivetrain
    Drive mDrive = new Drive();
    // true = use gyros and reset the encoders
    mDrive.init(this, true);
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    // Lets go
    telemetry.setAutoClear(false);
    telemetry.addData("Ready to Start", "");
    telemetry.update();
    waitForStart();
    telemetry.setAutoClear(true);
    // New Drive Controller
    TrajectoryDriveController controller = new TrajectoryDriveController(mDrive, defaultFollowerConfig);
    controller.loadProfile(ref, ref, 1, 1);
    ElapsedTime dtTimer = new ElapsedTime();
    while (opModeIsActive() && !controller.onTarget()) {
        // data
        telemetry.addData("kV", Constants.v.getDouble());
        telemetry.addData("kA", Constants.a.getDouble());
        telemetry.addData("Left Distance", mDrive.getLeftDistanceInches());
        telemetry.addData("Right Distance", mDrive.getRightDistanceInches());
        telemetry.addData("Left Speed", controller.wantedLeftSpeed);
        telemetry.addData("Right Speed", controller.wantedRightSpeed);
        telemetry.addData("Speed output", controller.getGoal());
        telemetry.addData("Gyro", mDrive.getGyroAngleDegrees());
        telemetry.addData("dt", dtTimer.seconds());
        telemetry.addData("Number of Segments", controller.getNumSegments());
        telemetry.addData("Current Pos", controller.getFollowerCurrentSegment());
        telemetry.update();
        // Make sure we only update when we told the controller/trajectory we would
        if (dtTimer.seconds() > defaultTrajectoryConfig.dt) {
            controller.update();
            idle();
            dtTimer.reset();
        }
    }
}

Example 45 with Trajectory

use of com.team254.lib_2014.trajectory.Trajectory in project Relic_Main by TeamOverdrive.

the class TrajectoryGraph method main.

public static void main(String... args) {
    TrajectoryGenerator.Config defaultTrajectoryConfig = new TrajectoryGenerator.Config();
    // In/s
    defaultTrajectoryConfig.max_vel = 23.832;
    // In/s^2
    defaultTrajectoryConfig.max_acc = 40;
    // In/s^3
    defaultTrajectoryConfig.max_jerk = 40;
    // seconds, change of time in each update
    defaultTrajectoryConfig.dt = 0.01;
    Trajectory trajectory = TrajectoryGenerator.generate(defaultTrajectoryConfig, TrajectoryGenerator.TrapezoidalStrategy, 0, 0, 15, 0, 0);
    System.out.println(trajectory.toString());
}