Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-22 01:11:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add solvePNP, 3d tab on the UI, and some other misc bug fixes (#35)

Browse Source 
* Rebase solvePNP on master

* added 3D tab minimap and csv reader

* More solvePNP

* Create draw pipe for pnp data

* SolvePNP piping work

* Move sorting into solvepnppipe

* Create calibration pipeline

* Update CalibrateSolvePNPPipeline.java

* add camera tilt angle

* Add calibration slider and snapshot button to 3D view

* Mirror updates in the socket handler

* add 3d calibration mode to the pipeline manager

* created calibration functions in ui and backend

* Start plumbing calibration

* Add snapshot and other handling to the RequestHandler

* added select resolution before starting calibration

* Rename solvePNPPipe to bounding box solve pnp pipe

* Update BoundingBoxSolvePNPPipe.java

* Add Mat serializer and CameraCalibrationConfig

* Begun calibration saving, fixed UI/Backend snapshot count mismatch

* Add (unplumbed) option to set checkerboard size

This will allow users to change the units their calibration is in

* Create chessboard.png

* Fix calibration NPE

* changed string serialization to a json send

* bug fixed cancellation button

* Fix spelling of snapshot in 3d.vue

* Plumb resolution change

* Set resolution during config, start on config serialization

* Update .gitignore

* Config fixes

* Start transition away from cvpipeline3d

* fix NPE on uncalibrated cameras

* clear list on fail

* Fix video mode index error

* ignore getters in camera calibration config

* Create json constructor for jsonmat

* get solvePNP mostly returning sane values

* Fix solvePNP bug and add unit test

* FIx calibration mat truncation

* added capture amount model upload and minimap data

* Standardize on meters in calibration and bounding box

* fix json out of bounds and handle null calibration more gracefully

* don't put text on calibrate image, go back to inches

* convert distance to meters

this means calibration will need to be in inches

* Actually save raw contor

* Update GroupContoursPipe.java

* Add all calibration return to camera capture

* hard code 2019 target

* bugfixed draw2d added fail calib popup, merge end and cancel

added the res index to the calib start

* Clarify error message and draw more fancy rectangles

* Cleanup memory in solvepnp

* re did minimap component

* fix npe if left/right is null

* remove references to 2d

* try-catch running the current pipeline

* Add method to find corners using the harris corner detector

* Possibly fix left/right missmatch

* Fix 3D Tab error

* FIx file permissions, mat serializer adjustments

* fixed mini map for field coordinates

* mini map changes fov

* Update SolvePNPPipe.java

* get rid of target corners

* some memory leak fixes

* fixed mini map location

* added position under minimap

* changed player fov look

* put all targets in the web send

* re did target send to ui added target tables, bugfix calibration

* fixed y position

* Add tilt angle to capture properties

* maybe fix y axis in minimap

* Add square size to onCalibrationEnding

* Possibly add square size to UI

* fix NPE with pitch

* Fix bug with sending multiple targets

* Only instantiate 3d stuff if we are in 3d mode

* Fix array list exceptions

* Fix bug in sort contors

list was truncated too early

* added download chess, tilt setting and ordinal tilt,

* added square size connection

* removed unused code

* Update pom version to 2.1-RELEASE

* Send camera calibrations to UI

* Stream pose list to a LIst

* Only stream necessary parts of the aux list entry

* Make broadcastMessage synchronized to prevent ConcurrentModificationExceptions

* added fps counter changed squaresize steps bug fixes in tables

* bugfix camera settings cam wont change

Authored-by: oriagranat9 <oriagranat9@gmail.com>
This commit is contained in:
Matt
2019-12-31 04:53:20 -08:00
committed by oriagranat9
parent d8c027dae7
commit 1decd2c3d7
70 changed files with 3029 additions and 297 deletions
Download Patch File Download Diff File Expand all files Collapse all files

271
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java Normal file
View File

@@ -0,0 +1,271 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import java.io.IOException;
import java.util.Objects;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.DeserializationContext;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.SerializerProvider;
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
/**
* Represents a 2d pose containing translational and rotational elements.
*/
@JsonSerialize(using = Pose2d.PoseSerializer.class)
@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
public class Pose2d {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
/**
* Constructs a pose at the origin facing toward the positive X axis.
* (Translation2d{0, 0} and Rotation{0})
*/
public Pose2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
}
/**
* Constructs a pose with the specified translation and rotation.
*
* @param translation The translational component of the pose.
* @param rotation The rotational component of the pose.
*/
public Pose2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Convenience constructors that takes in x and y values directly instead of
* having to construct a Translation2d.
*
* @param x The x component of the translational component of the pose.
* @param y The y component of the translational component of the pose.
* @param rotation The rotational component of the pose.
*/
@SuppressWarnings("ParameterName")
public Pose2d(double x, double y, Rotation2d rotation) {
m_translation = new Translation2d(x, y);
m_rotation = rotation;
}
/**
* Transforms the pose by the given transformation and returns the new
* transformed pose.
*
* <p>The matrix multiplication is as follows
* [x_new] [cos, -sin, 0][transform.x]
* [y_new] += [sin, cos, 0][transform.y]
* [t_new] [0, 0, 1][transform.t]
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d plus(Transform2d other) {
return transformBy(other);
}
/**
* Returns the Transform2d that maps the one pose to another.
*
* @param other The initial pose of the transformation.
* @return The transform that maps the other pose to the current pose.
*/
public Transform2d minus(Pose2d other) {
final var pose = this.relativeTo(other);
return new Transform2d(pose.getTranslation(), pose.getRotation());
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the pose.
*/
public Translation2d getTranslation() {
return m_translation;
}
/**
* Returns the rotational component of the transformation.
*
* @return The rotational component of the pose.
*/
public Rotation2d getRotation() {
return m_rotation;
}
/**
* Transforms the pose by the given transformation and returns the new pose.
* See + operator for the matrix multiplication performed.
*
* @param other The transform to transform the pose by.
* @return The transformed pose.
*/
public Pose2d transformBy(Transform2d other) {
return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
m_rotation.plus(other.getRotation()));
}
/**
* Returns the other pose relative to the current pose.
*
* <p>This function can often be used for trajectory tracking or pose
* stabilization algorithms to get the error between the reference and the
* current pose.
*
* @param other The pose that is the origin of the new coordinate frame that
* the current pose will be converted into.
* @return The current pose relative to the new origin pose.
*/
public Pose2d relativeTo(Pose2d other) {
var transform = new Transform2d(other, this);
return new Pose2d(transform.getTranslation(), transform.getRotation());
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/state-space-guide.pdf">
* Controls Engineering in the FIRST Robotics Competition</a>
* section on nonlinear pose estimation for derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the
* previous pose update. When the user runs exp() on the previous known
* field-relative pose with the argument being the twist, the user will
* receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential
* equation moving the pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the
* previous pose update. For example, if a non-holonomic robot moves forward
* 0.01 meters and changes angle by 0.5 degrees since the previous pose update,
* the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
* @return The new pose of the robot.
*/
@SuppressWarnings("LocalVariableName")
public Pose2d exp(Twist2d twist) {
double dx = twist.dx;
double dy = twist.dy;
double dtheta = twist.dtheta;
double sinTheta = Math.sin(dtheta);
double cosTheta = Math.cos(dtheta);
double s;
double c;
if (Math.abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
return this.plus(transform);
}
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output
* of a.Log(b), then a.Exp(c) would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
public Twist2d log(Pose2d end) {
final var transform = end.relativeTo(this);
final var dtheta = transform.getRotation().getRadians();
final var halfDtheta = dtheta / 2.0;
final var cosMinusOne = transform.getRotation().getCos() - 1;
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
Translation2d translationPart = transform.getTranslation().rotateBy(
new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
@Override
public String toString() {
return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Pose2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Pose2d) {
return ((Pose2d) obj).m_translation.equals(m_translation)
&& ((Pose2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
static class PoseSerializer extends StdSerializer<Pose2d> {
PoseSerializer() {
super(Pose2d.class);
}
@Override
public void serialize(
Pose2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeObjectField("translation", value.m_translation);
jgen.writeObjectField("rotation", value.m_rotation);
jgen.writeEndObject();
}
}
static class PoseDeserializer extends StdDeserializer<Pose2d> {
PoseDeserializer() {
super(Pose2d.class);
}
@Override
public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
Translation2d translation =
jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
return new Pose2d(translation, rotation);
}
}
}

251
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java Normal file
View File

@@ -0,0 +1,251 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import java.io.IOException;
import java.util.Objects;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.DeserializationContext;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.SerializerProvider;
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
/**
* A rotation in a 2d coordinate frame represented a point on the unit circle
* (cosine and sine).
*/
@JsonSerialize(using = Rotation2d.RotationSerializer.class)
@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
public class Rotation2d {
private final double m_value;
private final double m_cos;
private final double m_sin;
/**
* Constructs a Rotation2d with a default angle of 0 degrees.
*/
public Rotation2d() {
m_value = 0.0;
m_cos = 1.0;
m_sin = 0.0;
}
/**
* Constructs a Rotation2d with the given radian value.
* The x and y don't have to be normalized.
*
* @param value The value of the angle in radians.
*/
public Rotation2d(double value) {
m_value = value;
m_cos = Math.cos(value);
m_sin = Math.sin(value);
}
/**
* Constructs a Rotation2d with the given x and y (cosine and sine)
* components.
*
* @param x The x component or cosine of the rotation.
* @param y The y component or sine of the rotation.
*/
@SuppressWarnings("ParameterName")
public Rotation2d(double x, double y) {
double magnitude = Math.hypot(x, y);
if (magnitude > 1e-6) {
m_sin = y / magnitude;
m_cos = x / magnitude;
} else {
m_sin = 0.0;
m_cos = 1.0;
}
m_value = Math.atan2(m_sin, m_cos);
}
/**
* Constructs and returns a Rotation2d with the given degree value.
*
* @param degrees The value of the angle in degrees.
* @return The rotation object with the desired angle value.
*/
public static Rotation2d fromDegrees(double degrees) {
return new Rotation2d(Math.toRadians(degrees));
}
/**
* Adds two rotations together, with the result being bounded between -pi and
* pi.
*
* <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
* Rotation2d{-pi/2}
*
* @param other The rotation to add.
* @return The sum of the two rotations.
*/
public Rotation2d plus(Rotation2d other) {
return rotateBy(other);
}
/**
* Subtracts the new rotation from the current rotation and returns the new
* rotation.
*
* <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
* Rotation2d{-pi/2}
*
* @param other The rotation to subtract.
* @return The difference between the two rotations.
*/
public Rotation2d minus(Rotation2d other) {
return rotateBy(other.unaryMinus());
}
/**
* Takes the inverse of the current rotation. This is simply the negative of
* the current angular value.
*
* @return The inverse of the current rotation.
*/
public Rotation2d unaryMinus() {
return new Rotation2d(-m_value);
}
/**
* Multiplies the current rotation by a scalar.
*
* @param scalar The scalar.
* @return The new scaled Rotation2d.
*/
public Rotation2d times(double scalar) {
return new Rotation2d(m_value * scalar);
}
/**
* Adds the new rotation to the current rotation using a rotation matrix.
*
* <p>The matrix multiplication is as follows:
* [cos_new] [other.cos, -other.sin][cos]
* [sin_new] = [other.sin, other.cos][sin]
* value_new = atan2(cos_new, sin_new)
*
* @param other The rotation to rotate by.
* @return The new rotated Rotation2d.
*/
public Rotation2d rotateBy(Rotation2d other) {
return new Rotation2d(
m_cos * other.m_cos - m_sin * other.m_sin,
m_cos * other.m_sin + m_sin * other.m_cos
);
}
/*
* Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
*/
public double getRadians() {
return m_value;
}
/**
* Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
public double getDegrees() {
return Math.toDegrees(m_value);
}
/**
* Returns the cosine of the rotation.
*
* @return The cosine of the rotation.
*/
public double getCos() {
return m_cos;
}
/**
* Returns the sine of the rotation.
*
* @return The sine of the rotation.
*/
public double getSin() {
return m_sin;
}
/**
* Returns the tangent of the rotation.
*
* @return The tangent of the rotation.
*/
public double getTan() {
return m_sin / m_cos;
}
@Override
public String toString() {
return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
}
/**
* Checks equality between this Rotation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Rotation2d) {
return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_value);
}
static class RotationSerializer extends StdSerializer<Rotation2d> {
RotationSerializer() {
super(Rotation2d.class);
}
@Override
public void serialize(
Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeNumberField("radians", value.m_value);
jgen.writeEndObject();
}
}
static class RotationDeserializer extends StdDeserializer<Rotation2d> {
RotationDeserializer() {
super(Rotation2d.class);
}
@Override
public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
double radians = node.get("radians").numberValue().doubleValue();
return new Rotation2d(radians);
}
}
}

106
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java Normal file
View File

@@ -0,0 +1,106 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* Represents a transformation for a Pose2d.
*/
public class Transform2d {
private final Translation2d m_translation;
private final Rotation2d m_rotation;
/**
* Constructs the transform that maps the initial pose to the final pose.
*
* @param initial The initial pose for the transformation.
* @param last The final pose for the transformation.
*/
public Transform2d(Pose2d initial, Pose2d last) {
// We are rotating the difference between the translations
// using a clockwise rotation matrix. This transforms the global
// delta into a local delta (relative to the initial pose).
m_translation = last.getTranslation().minus(initial.getTranslation())
.rotateBy(initial.getRotation().unaryMinus());
m_rotation = last.getRotation().minus(initial.getRotation());
}
/**
* Constructs a transform with the given translation and rotation components.
*
* @param translation Translational component of the transform.
* @param rotation Rotational component of the transform.
*/
public Transform2d(Translation2d translation, Rotation2d rotation) {
m_translation = translation;
m_rotation = rotation;
}
/**
* Constructs the identity transform -- maps an initial pose to itself.
*/
public Transform2d() {
m_translation = new Translation2d();
m_rotation = new Rotation2d();
}
/**
* Scales the transform by the scalar.
*
* @param scalar The scalar.
* @return The scaled Transform2d.
*/
public Transform2d times(double scalar) {
return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
}
/**
* Returns the translation component of the transformation.
*
* @return The translational component of the transform.
*/
public Translation2d getTranslation() {
return m_translation;
}
/**
* Returns the rotational component of the transformation.
*
* @return Reference to the rotational component of the transform.
*/
public Rotation2d getRotation() {
return m_rotation;
}
@Override
public String toString() {
return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
}
/**
* Checks equality between this Transform2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Transform2d) {
return ((Transform2d) obj).m_translation.equals(m_translation)
&& ((Transform2d) obj).m_rotation.equals(m_rotation);
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_translation, m_rotation);
}
}

243
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java Normal file
View File

@@ -0,0 +1,243 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import java.io.IOException;
import java.util.Objects;
import com.fasterxml.jackson.core.JsonGenerator;
import com.fasterxml.jackson.core.JsonParser;
import com.fasterxml.jackson.core.JsonProcessingException;
import com.fasterxml.jackson.databind.DeserializationContext;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.SerializerProvider;
import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
import com.fasterxml.jackson.databind.annotation.JsonSerialize;
import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
import com.fasterxml.jackson.databind.ser.std.StdSerializer;
/**
* Represents a translation in 2d space.
* This object can be used to represent a point or a vector.
*
* <p>This assumes that you are using conventional mathematical axes.
* When the robot is placed on the origin, facing toward the X direction,
* moving forward increases the X, whereas moving to the left increases the Y.
*/
@JsonSerialize(using = Translation2d.TranslationSerializer.class)
@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
@SuppressWarnings({"ParameterName", "MemberName"})
public class Translation2d {
private final double m_x;
private final double m_y;
/**
* Constructs a Translation2d with X and Y components equal to zero.
*/
public Translation2d() {
this(0.0, 0.0);
}
/**
* Constructs a Translation2d with the X and Y components equal to the
* provided values.
*
* @param x The x component of the translation.
* @param y The y component of the translation.
*/
public Translation2d(double x, double y) {
m_x = x;
m_y = y;
}
/**
* Calculates the distance between two translations in 2d space.
*
* <p>This function uses the pythagorean theorem to calculate the distance.
* distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
*
* @param other The translation to compute the distance to.
* @return The distance between the two translations.
*/
public double getDistance(Translation2d other) {
return Math.hypot(other.m_x - m_x, other.m_y - m_y);
}
/**
* Returns the X component of the translation.
*
* @return The x component of the translation.
*/
public double getX() {
return m_x;
}
/**
* Returns the Y component of the translation.
*
* @return The y component of the translation.
*/
public double getY() {
return m_y;
}
/**
* Returns the norm, or distance from the origin to the translation.
*
* @return The norm of the translation.
*/
public double getNorm() {
return Math.hypot(m_x, m_y);
}
/**
* Applies a rotation to the translation in 2d space.
*
* <p>This multiplies the translation vector by a counterclockwise rotation
* matrix of the given angle.
* [x_new] [other.cos, -other.sin][x]
* [y_new] = [other.sin, other.cos][y]
*
* <p>For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
* Translation2d of {0, 2}.
*
* @param other The rotation to rotate the translation by.
* @return The new rotated translation.
*/
public Translation2d rotateBy(Rotation2d other) {
return new Translation2d(
m_x * other.getCos() - m_y * other.getSin(),
m_x * other.getSin() + m_y * other.getCos()
);
}
/**
* Adds two translations in 2d space and returns the sum. This is similar to
* vector addition.
*
* <p>For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
* Translation2d{3.0, 8.0}
*
* @param other The translation to add.
* @return The sum of the translations.
*/
public Translation2d plus(Translation2d other) {
return new Translation2d(m_x + other.m_x, m_y + other.m_y);
}
/**
* Subtracts the other translation from the other translation and returns the
* difference.
*
* <p>For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
* Translation2d{4.0, 2.0}
*
* @param other The translation to subtract.
* @return The difference between the two translations.
*/
public Translation2d minus(Translation2d other) {
return new Translation2d(m_x - other.m_x, m_y - other.m_y);
}
/**
* Returns the inverse of the current translation. This is equivalent to
* rotating by 180 degrees, flipping the point over both axes, or simply
* negating both components of the translation.
*
* @return The inverse of the current translation.
*/
public Translation2d unaryMinus() {
return new Translation2d(-m_x, -m_y);
}
/**
* Multiplies the translation by a scalar and returns the new translation.
*
* <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
*
* @param scalar The scalar to multiply by.
* @return The scaled translation.
*/
public Translation2d times(double scalar) {
return new Translation2d(m_x * scalar, m_y * scalar);
}
/**
* Divides the translation by a scalar and returns the new translation.
*
* <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
*
* @param scalar The scalar to multiply by.
* @return The reference to the new mutated object.
*/
public Translation2d div(double scalar) {
return new Translation2d(m_x / scalar, m_y / scalar);
}
@Override
public String toString() {
return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
}
public static Translation2d fromRotation2d(Rotation2d rotation) {
return new Translation2d(rotation.getCos(), rotation.getSin());
}
/**
* Checks equality between this Translation2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Translation2d) {
return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
&& Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(m_x, m_y);
}
static class TranslationSerializer extends StdSerializer<Translation2d> {
TranslationSerializer() {
super(Translation2d.class);
}
@Override
public void serialize(
Translation2d value, JsonGenerator jgen, SerializerProvider provider)
throws IOException, JsonProcessingException {
jgen.writeStartObject();
jgen.writeNumberField("x", value.m_x);
jgen.writeNumberField("y", value.m_y);
jgen.writeEndObject();
}
}
static class TranslationDeserializer extends StdDeserializer<Translation2d> {
TranslationDeserializer() {
super(Translation2d.class);
}
@Override
public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
throws IOException, JsonProcessingException {
JsonNode node = jp.getCodec().readTree(jp);
double xval = node.get("x").numberValue().doubleValue();
double yval = node.get("y").numberValue().doubleValue();
return new Translation2d(xval, yval);
}
}
}

76
chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java Normal file
View File

@@ -0,0 +1,76 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.geometry;
import java.util.Objects;
/**
* A change in distance along arc since the last pose update. We can use ideas
* from differential calculus to create new Pose2ds from a Twist2d and vise
* versa.
*
* <p>A Twist can be used to represent a difference between two poses.
*/
@SuppressWarnings("MemberName")
public class Twist2d {
/**
* Linear "dx" component.
*/
public double dx;
/**
* Linear "dy" component.
*/
public double dy;
/**
* Angular "dtheta" component (radians).
*/
public double dtheta;
public Twist2d() {
}
/**
* Constructs a Twist2d with the given values.
* @param dx Change in x direction relative to robot.
* @param dy Change in y direction relative to robot.
* @param dtheta Change in angle relative to robot.
*/
public Twist2d(double dx, double dy, double dtheta) {
this.dx = dx;
this.dy = dy;
this.dtheta = dtheta;
}
@Override
public String toString() {
return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
}
/**
* Checks equality between this Twist2d and another object.
*
* @param obj The other object.
* @return Whether the two objects are equal or not.
*/
@Override
public boolean equals(Object obj) {
if (obj instanceof Twist2d) {
return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
&& Math.abs(((Twist2d) obj).dy - dy) < 1E-9
&& Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
}
return false;
}
@Override
public int hashCode() {
return Objects.hash(dx, dy, dtheta);
}
}