Examples with Trajectory edu.wpi.first.math.trajectory.Trajectory used on opensource projects

Example 26 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project FRC2022 by 2202Programming.

the class AutoPaths method buildEntry.

void buildEntry(Path file) {
    Path fn = file.getName(file.getNameCount() - 1);
    String key = fn.toString().split("\\.")[0];
    System.out.println("**Key:" + key);
    Trajectory traj = loadTrajectory(fn.toString());
    pathChooser.addOption(key, traj);
    // keep a map so we can access any path we need
    m_map.put(key, traj);
}

Example 27 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project PathWeaver by wpilibsuite.

the class WpilibSpline method writeToFile.

@Override
public boolean writeToFile(java.nio.file.Path path) {
    final AtomicBoolean okay = new AtomicBoolean(true);
    TrajectoryGenerator.setErrorHandler((error, stacktrace) -> {
        LOGGER.log(Level.WARNING, "Could not write Spline to file: " + error, stacktrace);
        okay.set(false);
    });
    try {
        var values = ProjectPreferences.getInstance().getValues();
        var prefs = ProjectPreferences.getInstance();
        var lengthUnit = prefs.getField().getUnit();
        double height = prefs.getField().getRealLength().getValue().doubleValue();
        var maxVelocity = values.getMaxVelocity();
        var maxAcceleration = values.getMaxAcceleration();
        var trackWidth = values.getTrackWidth();
        // If the export type is different (i.e. meters), then we have to convert it. Otherwise we are good.
        if (prefs.getValues().getExportUnit() == ProjectPreferences.ExportUnit.METER) {
            UnitConverter converter = lengthUnit.getConverterTo(PathUnits.METER);
            height = converter.convert(height);
            maxVelocity = converter.convert(maxVelocity);
            maxAcceleration = converter.convert(maxAcceleration);
            trackWidth = converter.convert(trackWidth);
        }
        TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAcceleration).setKinematics(new DifferentialDriveKinematics(trackWidth)).setReversed(waypoints.get(0).isReversed());
        Trajectory traj = trajectoryFromWaypoints(waypoints, config);
        for (int i = 0; i < traj.getStates().size(); ++i) {
            var st = traj.getStates().get(i);
            traj.getStates().set(i, new Trajectory.State(st.timeSeconds, st.velocityMetersPerSecond, st.accelerationMetersPerSecondSq, new Pose2d(st.poseMeters.getX(), st.poseMeters.getY() + height, st.poseMeters.getRotation()), st.curvatureRadPerMeter));
        }
        TrajectoryUtil.toPathweaverJson(traj, path.resolveSibling(path.getFileName() + ".wpilib.json"));
        return okay.get();
    } catch (IOException except) {
        LOGGER.log(Level.WARNING, "Could not write Spline to file", except);
        return false;
    }
}

Example 28 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project 2022 by Team5705.

the class RobotContainer method getAutonomousCommand.

// -----------------------------------------------------------------------------------------------------------------------------------------
public Command getAutonomousCommand() {
    // Create a voltage constraint to ensure we don't accelerate too fast
    var autoVoltageConstraint = new DifferentialDriveVoltageConstraint(new SimpleMotorFeedforward(pathWeaver.ksVolts, pathWeaver.kvVoltSecondsPerMeter, pathWeaver.kaVoltSecondsSquaredPerMeter), pathWeaver.kDriveKinematics, 10);
    // Create config for trajectory
    TrajectoryConfig config = new TrajectoryConfig(pathWeaver.kMaxSpeedMetersPerSecond, pathWeaver.kMaxAccelerationMetersPerSecondSquared).setKinematics(pathWeaver.kDriveKinematics).addConstraint(autoVoltageConstraint);
    // An example trajectory to follow.  All units in meters.
    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(// Start at the origin facing the +X direction
    new Pose2d(0, 0, new Rotation2d(0)), // Pass through these two interior waypoints, making an 's' curve path
    List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward
    new Pose2d(3, 0, new Rotation2d(0)), // Pass config
    config);
    RamseteCommand ramseteCommand = new RamseteCommand(// Trayectoria cargada desde pathweaver
    trajectory, powertrain::getPose, new RamseteController(pathWeaver.kRamseteB, pathWeaver.kRamseteZeta), new SimpleMotorFeedforward(pathWeaver.ksVolts, pathWeaver.kvVoltSecondsPerMeter, pathWeaver.kaVoltSecondsSquaredPerMeter), pathWeaver.kDriveKinematics, powertrain::getWheelSpeeds, new PIDController(pathWeaver.kPDriveVel, 0, 0), new PIDController(pathWeaver.kPDriveVel, 0, 0), // RamseteCommand passes volts to the callback
    powertrain::setVolts, powertrain);
    // Reset odometry to the starting pose of the trajectory.
    powertrain.resetOdometry(exampleTrajectory.getInitialPose());
    // Run path following command, then stop at the end.
    return ramseteCommand.andThen(() -> powertrain.setVolts(0, 0));
}

Example 29 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project Entropy2022 by Team138Entropy.

the class TrajectoryLibrary method get_Old_T2_B3.

public Trajectory get_Old_T2_B3() {
    Trajectory traj = null;
    String fileName = "Tarmac2.DEMO.wpilib.json";
    try {
        Path trajectoryPath = getTrajectoryPath(fileName);
        traj = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + fileName);
    }
    return traj;
}

Example 30 with Trajectory

use of edu.wpi.first.math.trajectory.Trajectory in project Entropy2022 by Team138Entropy.

the class TrajectoryLibrary method get_New_T1_B3.

public Trajectory get_New_T1_B3() {
    Trajectory traj = null;
    String fileName = "New-T1_B3path.wpilib.json";
    try {
        Path trajectoryPath = getTrajectoryPath(fileName);
        traj = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
    } catch (IOException ex) {
        System.out.println("Unable to open trajectory: " + fileName);
    }
    return traj;
}