use of edu.wpi.first.math.kinematics.ChassisSpeeds in project RoboCode2022 by HarkerRobo.
the class AlignWithLimelight method end.
public void end(boolean isFinished) {
ChassisSpeeds chassis = ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, Rotation2d.fromDegrees(Drivetrain.getInstance().getPigeon().getYaw()));
Drivetrain.getInstance().setAngleAndDriveVelocity(Drivetrain.getInstance().getKinematics().toSwerveModuleStates(chassis), false);
}
use of edu.wpi.first.math.kinematics.ChassisSpeeds in project 2022-RapidReact by Spartronics4915.
the class DrivetrainEstimatorTest method testEstimator.
@Test
public void testEstimator() {
var stateStdDevs = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.02, 0.02, 0.01);
var measurementStdDevs = new MatBuilder<>(Nat.N6(), Nat.N1()).fill(0.1, 0.1, 0.1, 0.05, 0.05, 0.002);
var est = new DrivetrainEstimator(stateStdDevs, measurementStdDevs, 3, new Pose2d());
final double dt = 0.01;
final double visionUpdateRate = 0.2;
var traj = TrajectoryGenerator.generateTrajectory(List.of(new Pose2d(), new Pose2d(3, 3, new Rotation2d())), new TrajectoryConfig(Units.inchesToMeters(12), Units.inchesToMeters(12)));
var kinematics = new DifferentialDriveKinematics(1);
Pose2d lastPose = null;
List<Double> trajXs = new ArrayList<>();
List<Double> trajYs = new ArrayList<>();
List<Double> observerXs = new ArrayList<>();
List<Double> observerYs = new ArrayList<>();
List<Double> slamXs = new ArrayList<>();
List<Double> slamYs = new ArrayList<>();
List<Double> visionXs = new ArrayList<>();
List<Double> visionYs = new ArrayList<>();
var rand = new Random();
final double steadyStateErrorX = 1.0;
final double steadyStateErrorY = 1.0;
double t = 0.0;
Pose2d lastVisionUpdate = null;
double lastVisionUpdateT = Double.NEGATIVE_INFINITY;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= traj.getTotalTimeSeconds()) {
t += dt;
var groundtruthState = traj.sample(t);
var input = kinematics.toWheelSpeeds(new ChassisSpeeds(groundtruthState.velocityMetersPerSecond, 0.0, // ds/dt * dtheta/ds = dtheta/dt
groundtruthState.velocityMetersPerSecond * groundtruthState.curvatureRadPerMeter));
Matrix<N3, N1> u = new MatBuilder<>(Nat.N3(), Nat.N1()).fill(input.leftMetersPerSecond * dt, input.rightMetersPerSecond * dt, 0.0);
if (lastPose != null) {
u.set(2, 0, groundtruthState.poseMeters.getRotation().getRadians() - lastPose.getRotation().getRadians());
}
u = u.plus(StateSpaceUtil.makeWhiteNoiseVector(new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.002, 0.002, 0.001)));
lastPose = groundtruthState.poseMeters;
Pose2d realPose = groundtruthState.poseMeters;
if (lastVisionUpdateT + visionUpdateRate < t) {
if (lastVisionUpdate != null) {
est.addVisionMeasurement(lastVisionUpdate, lastVisionUpdateT);
}
lastVisionUpdateT = t;
lastVisionUpdate = realPose.transformBy(new Transform2d(new Translation2d(rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.05), new Rotation2d(rand.nextGaussian() * 0.002)));
visionXs.add(lastVisionUpdate.getTranslation().getX());
visionYs.add(lastVisionUpdate.getTranslation().getY());
}
double dist = realPose.getTranslation().getDistance(new Translation2d());
Pose2d measurementVSlam = realPose.transformBy(new Transform2d(new Translation2d(steadyStateErrorX * (dist / 76.0), steadyStateErrorY * (dist / 76.0)), new Rotation2d())).transformBy(new Transform2d(new Translation2d(rand.nextGaussian() * 0.05, rand.nextGaussian() * 0.05), new Rotation2d(rand.nextGaussian() * 0.001)));
var xHat = est.update(measurementVSlam, u.get(0, 0), u.get(1, 0), u.get(2, 0), t);
double error = groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
trajXs.add(groundtruthState.poseMeters.getTranslation().getX());
trajYs.add(groundtruthState.poseMeters.getTranslation().getY());
observerXs.add(xHat.getTranslation().getX());
observerYs.add(xHat.getTranslation().getY());
slamXs.add(measurementVSlam.getTranslation().getX());
slamYs.add(measurementVSlam.getTranslation().getY());
}
System.out.println("Mean error (meters): " + errorSum / (traj.getTotalTimeSeconds() / dt));
System.out.println("Max error (meters): " + maxError);
try {
if (true)
throw new HeadlessException();
var chartBuilder = new XYChartBuilder();
chartBuilder.title = "The Magic of Sensor Fusion";
var chart = chartBuilder.build();
chart.addSeries("vSLAM", slamXs, slamYs);
chart.addSeries("Vision", visionXs, visionYs);
chart.addSeries("Trajectory", trajXs, trajYs);
chart.addSeries("xHat", observerXs, observerYs);
new SwingWrapper<>(chart).displayChart();
try {
Thread.sleep(1000000000);
} catch (InterruptedException e) {
}
} catch (java.awt.HeadlessException ex) {
System.out.println("skipping charts in headless mode");
}
}
use of edu.wpi.first.math.kinematics.ChassisSpeeds in project 2022-RapidReact by Spartronics4915.
the class T265Camera method consumePoseUpdate.
private synchronized void consumePoseUpdate(float x, float y, float radians, float dx, float dy, float dtheta, int confOrdinal) {
// First we apply an offset to go from the camera coordinate system to the
// robot coordinate system with an origin at the center of the robot. This
// is not a directional transformation.
// Then we transform the pose our camera is giving us so that it reports is
// the robot's pose, not the camera's. This is a directional transformation.
final Pose2d currentPose = new Pose2d(x - mRobotOffset.getTranslation().getX(), y - mRobotOffset.getTranslation().getY(), new Rotation2d(radians)).transformBy(mRobotOffset);
mLastRecievedPose = currentPose;
if (!mIsStarted)
return;
// See
// https://github.com/IntelRealSense/librealsense/blob/7f2ba0de8769620fd672f7b44101f0758e7e2fb3/include/librealsense2/h/rs_types.h#L115
// for ordinals
PoseConfidence confidence;
switch(confOrdinal) {
case 0x0:
confidence = PoseConfidence.Failed;
break;
case 0x1:
confidence = PoseConfidence.Low;
break;
case 0x2:
confidence = PoseConfidence.Medium;
break;
case 0x3:
confidence = PoseConfidence.High;
break;
default:
throw new RuntimeException("Unknown confidence ordinal \"" + confOrdinal + "\" passed from native code");
}
final Pose2d transformedPose = mOrigin.transformBy(new Transform2d(currentPose.getTranslation(), currentPose.getRotation()));
mPoseConsumer.accept(new CameraUpdate(transformedPose, new ChassisSpeeds(dx, dy, dtheta), confidence));
}
use of edu.wpi.first.math.kinematics.ChassisSpeeds in project Pathfinder2 by Wobblyyyy.
the class TestCompareChassisSpeedsWithTranslation method compare.
private static void compare(ChassisSpeeds speeds, Translation translation) {
ChassisSpeeds fromTranslation = WPIAdapter.speedsFromTranslation(translation);
Assertions.assertEquals(translation, WPIAdapter.translationFromSpeeds(speeds));
AssertionUtils.assertSoftEquals(speeds.vxMetersPerSecond, fromTranslation.vxMetersPerSecond, 0.01);
AssertionUtils.assertSoftEquals(speeds.vyMetersPerSecond, fromTranslation.vyMetersPerSecond, 0.01);
AssertionUtils.assertSoftEquals(speeds.omegaRadiansPerSecond, fromTranslation.omegaRadiansPerSecond, 0.01);
}
use of edu.wpi.first.math.kinematics.ChassisSpeeds in project RapidReact2022 by team223.
the class AutoDrive1 method execute.
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
RamseteController controller = new RamseteController();
double currentTime = (double) (System.currentTimeMillis() - startTimeMillis) / 1000;
Trajectory.State goal = trajectory.sample(currentTime);
ChassisSpeeds updatedSpeeds = controller.calculate(RobotContainer.driveSubsystem.getPosition(), goal);
DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(updatedSpeeds);
double left = wheelSpeeds.leftMetersPerSecond;
SmartDashboard.putNumber("Target X", goal.poseMeters.getX());
SmartDashboard.putNumber("Target Y", goal.poseMeters.getY());
SmartDashboard.putNumber("Actual X", RobotContainer.driveSubsystem.getPosition().getX());
SmartDashboard.putNumber("Actual Y", RobotContainer.driveSubsystem.getPosition().getY());
double right = wheelSpeeds.rightMetersPerSecond;
double leftSpeed = RobotContainer.driveSubsystem.getLeftEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
double rightSpeed = RobotContainer.driveSubsystem.getRightEncoderVelocity() * DriveSubsystem.rotToMeters / 60;
System.out.println(leftSpeed + "vs" + left);
RobotContainer.driveSubsystem.setMotors(drivePid.calculate(leftSpeed, left), drivePid.calculate(rightSpeed, right));
}
Aggregations